00001 #include "MotionManager.h"
00002 #include "Shared/debuget.h"
00003 #include "Shared/WorldState.h"
00004 #include "Events/EventRouter.h"
00005 #include "Shared/StackTrace.h"
00006 #include "Shared/ProjectInterface.h"
00007
00008 #ifndef PLATFORM_APERIOS
00009 # include "IPC/MessageQueue.h"
00010 # include "IPC/MessageReceiver.h"
00011 # include "Shared/ProjectInterface.h"
00012 #endif
00013
00014 #include "Shared/ERS210Info.h"
00015 #include "Shared/ERS220Info.h"
00016 #include "Shared/ERS7Info.h"
00017 #include "Shared/Config.h"
00018
00019 #include <list>
00020
00021 MotionManager * motman=NULL;
00022 int MotionManager::_MMaccID[ProcessID::NumProcesses];
00023 EventTranslator* MotionManager::etrans=NULL;
00024
00025 const float MotionManager::kIgnoredPriority =-1;
00026 const float MotionManager::kBackgroundPriority = 0;
00027 const float MotionManager::kLowPriority = 5;
00028 const float MotionManager::kStdPriority = 10;
00029 const float MotionManager::kHighPriority = 50;
00030 const float MotionManager::kEmergencyPriority = 100;
00031
00032 using namespace std;
00033
00034
00035 typedef unsigned int uint;
00036
00037 MotionManager::MotionManager()
00038 : pidchanges(),cmdlist(),cur_cmd(invalid_MC_ID),MMlock(),numAcc(0)
00039 {
00040 for(uint x=0; x<NumOutputs; x++)
00041 cmdSums[x]=0;
00042 }
00043
00044 #ifdef PLATFORM_APERIOS
00045
00046 void
00047 MotionManager::InitAccess(OSubject* subj) {
00048 if(numAcc==MAX_ACCESS) {
00049 printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00050 return;
00051 }
00052 _MMaccID[ProcessID::getID()]=numAcc++;
00053
00054 MMlock.lock(getAccID());
00055
00056 subjs[getAccID()]=subj;
00057 if(cmdlist.size()>0)
00058 cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00059 MMlock.unlock();
00060 }
00061
00062 #else //now PLATFORM_LOCAL
00063
00064 void
00065 MotionManager::InitAccess(MessageQueueBase& mcbufq, Resource& behaviorLock) {
00066 if(numAcc==MAX_ACCESS) {
00067 printf("*** ERROR *** attempted to register more accessors with MotionManager than allowed by MAX_ACCESS\n");
00068 return;
00069 }
00070 _MMaccID[ProcessID::getID()]=numAcc++;
00071
00072 MMlock.lock(getAccID());
00073 subjs[getAccID()]=&mcbufq;
00074 mcrecvs[getAccID()]=new MessageReceiver(*subjs[getAccID()],receivedMsg);
00075 procLocks[getAccID()]=&behaviorLock;
00076 if(cmdlist.size()>0)
00077 cout << "*** WARNING *** MOTIONS ADDED BEFORE ALL INITACCESSED" << endl;
00078 MMlock.unlock();
00079 }
00080
00081 #endif //PLATFORM-specific initialization
00082
00083 void
00084 MotionManager::RemoveAccess() {
00085 #ifndef PLATFORM_APERIOS
00086
00087
00088 mcrecvs[getAccID()]->finish();
00089 delete mcrecvs[getAccID()];
00090 mcrecvs[getAccID()]=NULL;
00091 #endif
00092
00093 func_begin();
00094 for(MC_ID mc_id=cmdlist.begin(); mc_id!=cmdlist.end(); mc_id=cmdlist.next(mc_id)) {
00095 if(cmdlist[mc_id].rcr[getAccID()]!=NULL) {
00096 MotionCommand* mc=checkoutMotion(mc_id,true);
00097 int found=0;
00098 for(unsigned int i=0; i<numAcc; i++) {
00099 if(cmdlist[mc_id].rcr[i]!=NULL) {
00100 found++;
00101 }
00102 }
00103 cout << "Warning: " << ProcessID::getIDStr() << " dropping motion command " << mc_id << ", was active at shutdown " << (mc->getAutoPrune()?"(was set for autoprune)":"(leaked?)") << endl;
00104 #ifdef PLATFORM_APERIOS
00105 int refs=1;
00106 #else
00107 int refs=cmdlist[mc_id].rcr[getAccID()]->NumberOfLocalReference();
00108 #endif
00109 if(refs>1)
00110 cout << "Warning: " << ProcessID::getIDStr() << " still had " << refs-1 << " references to motion command " << mc_id << " as it was being dropped (these are now invalid)" << endl;
00111 for(int i=0; i<refs; ++i)
00112 cmdlist[mc_id].rcr[getAccID()]->RemoveReference();
00113 cmdlist[mc_id].rcr[getAccID()]=NULL;
00114 if(found==1) {
00115 MC_ID p=cmdlist.prev(mc_id);
00116 push_free(mc_id);
00117 mc_id=p;
00118 } else {
00119 checkinMotion(mc_id);
00120 }
00121 }
00122 }
00123 func_end();
00124 }
00125
00126
00127 MotionManager::~MotionManager() {
00128 if(!cmdlist.empty()) {
00129 func_begin();
00130 cout << "WARNING: MotionManager destruction with MotionCommands still attached." << endl;
00131 while(!cmdlist.empty()) {
00132 MC_ID mc_id=cmdlist.begin();
00133 for(unsigned int i=0; i<numAcc; i++)
00134 if(cmdlist[mc_id].rcr[i]!=NULL)
00135 cout << "MC " << mc_id << " was still referenced by InitAccess caller #" << getAccID() << endl;
00136 push_free(mc_id);
00137 }
00138 func_end();
00139 }
00140 }
00141
00142 void
00143 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd) {
00144 if(output >= NumOutputs)
00145 return;
00146 if(cmd.weight<=0)
00147 return;
00148
00149 if(caller==NULL || caller->getID()!=cur_cmd)
00150 func_begin();
00151 if(cur_cmd==invalid_MC_ID) {
00152 cmdSums[output]=cmd.value;
00153 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00154 cmdstatelist_t& curstatelist=cmdstates[output];
00155 cmdstatelist_t::index_t ent=curstatelist.begin();
00156 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00157 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd));
00158 else
00159 for(unsigned int i=0; i<NumFrames; i++)
00160 curstatelist[ent].frames[i]=cmd;
00161 }
00162 if(caller==NULL || caller->getID()!=cur_cmd)
00163 func_end();
00164 }
00165
00166 void
00167 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame) {
00168 if(output >= NumOutputs)
00169 return;
00170 if(cmd.weight<=0)
00171 return;
00172
00173 if(caller==NULL || caller->getID()!=cur_cmd)
00174 func_begin();
00175 if(cur_cmd==invalid_MC_ID) {
00176 cmdSums[output]=cmd.value;
00177 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00178 cmdstatelist_t& curstatelist=cmdstates[output];
00179 cmdstatelist_t::index_t ent=curstatelist.begin();
00180 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00181 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,frame));
00182 else
00183 curstatelist[ent].frames[frame]=cmd;
00184 }
00185 if(caller==NULL || caller->getID()!=cur_cmd)
00186 func_end();
00187 }
00188
00189 void
00190 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames]) {
00191 if(output >= NumOutputs)
00192 return;
00193 unsigned int hasWeight=NumFrames;
00194 for(unsigned int i=NumFrames-1; i!=-1U; i--)
00195 if(ocmds[i].weight>0) {
00196 hasWeight=i;
00197 break;
00198 }
00199 if(hasWeight==NumFrames)
00200 return;
00201
00202
00203 if(caller==NULL || caller->getID()!=cur_cmd)
00204 func_begin();
00205 if(cur_cmd==invalid_MC_ID) {
00206 cmdSums[output]=ocmds[hasWeight].value;
00207 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00208 cmdstatelist_t& curstatelist=cmdstates[output];
00209 cmdstatelist_t::index_t ent=curstatelist.begin();
00210 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00211 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds));
00212 else
00213 for(unsigned int i=0; i<NumFrames; i++)
00214 curstatelist[ent].frames[i]=ocmds[i];
00215 }
00216 if(caller==NULL || caller->getID()!=cur_cmd)
00217 func_end();
00218 }
00219
00220 void
00221 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid) {
00222 if(output >= NumOutputs)
00223 return;
00224
00225 if(caller==NULL || caller->getID()!=cur_cmd)
00226 func_begin();
00227 if(cur_cmd==invalid_MC_ID) {
00228 setPID(output,pid.pid);
00229 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00230 cmdstatelist_t& curstatelist=cmdstates[output];
00231 cmdstatelist_t::index_t ent=curstatelist.begin();
00232 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00233 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,pid));
00234 else
00235 curstatelist[ent].pid=pid;
00236 }
00237 if(caller==NULL || caller->getID()!=cur_cmd)
00238 func_end();
00239 }
00240
00241 void
00242 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid) {
00243 if(output >= NumOutputs)
00244 return;
00245
00246 if(caller==NULL || caller->getID()!=cur_cmd)
00247 func_begin();
00248 if(cur_cmd==invalid_MC_ID) {
00249 if(cmd.weight>0)
00250 cmdSums[output]=cmd.value;
00251 setPID(output,pid.pid);
00252 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00253 cmdstatelist_t& curstatelist=cmdstates[output];
00254 cmdstatelist_t::index_t ent=curstatelist.begin();
00255 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00256 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,cmd,pid));
00257 else {
00258 for(unsigned int i=0; i<NumFrames; i++)
00259 curstatelist[ent].frames[i]=cmd;
00260 curstatelist[ent].pid=pid;
00261 }
00262 }
00263 if(caller==NULL || caller->getID()!=cur_cmd)
00264 func_end();
00265 }
00266
00267 void
00268 MotionManager::setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd ocmds[NumFrames], const OutputPID& pid) {
00269 if(output >= NumOutputs)
00270 return;
00271
00272 if(caller==NULL || caller->getID()!=cur_cmd)
00273 func_begin();
00274 if(cur_cmd==invalid_MC_ID) {
00275 if(ocmds[NumFrames-1].weight>0)
00276 cmdSums[output]=ocmds[NumFrames-1].value;
00277 setPID(output,pid.pid);
00278 } else if(getPriority(cur_cmd)>=kBackgroundPriority) {
00279 cmdstatelist_t& curstatelist=cmdstates[output];
00280 cmdstatelist_t::index_t ent=curstatelist.begin();
00281 if(ent==curstatelist.end() || curstatelist[ent].mcid!=cur_cmd)
00282 curstatelist.push_front(OutputState(output,getPriority(cur_cmd),cur_cmd,ocmds,pid));
00283 else {
00284 for(unsigned int i=0; i<NumFrames; i++)
00285 curstatelist[ent].frames[i]=ocmds[i];
00286 curstatelist[ent].pid=pid;
00287 }
00288 }
00289 if(caller==NULL || caller->getID()!=cur_cmd)
00290 func_end();
00291 }
00292
00293
00294
00295
00296 void
00297 MotionManager::getOutputs(float outputs[][NumOutputs]) {
00298
00299
00300 if(state==NULL) {
00301
00302 for(uint f=0;f<NumFrames;f++)
00303 for(uint i=0; i<NumOutputs; i++)
00304 outputs[f][i]=0;
00305 for(uint f=0;f<NumFrames;f++)
00306 for(uint l=0; l<NumLEDs; l++)
00307 outputs[f][l]=l/(NumLEDs-1.0);
00308
00309
00310 return;
00311 }
00312 func_begin();
00313
00314
00315
00316 for(uint output=0; output<NumOutputs; output++)
00317 cmdstates[output].clear();
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330 for(uint output=0; output<NumPIDJoints; output++)
00331 if(state->pids[output][0]==0 && state->pids[output][1]==0 && state->pids[output][2]==0)
00332 cmdSums[output]=state->outputs[output];
00333
00334
00335 std::list<MC_ID> unlocked;
00336 for(MC_ID it=begin(); it!=end(); it=next(it))
00337 if(cmdlist[it].lastAccessor!=(accID_t)-1)
00338 unlocked.push_back(it);
00339 unsigned int lastProcessed=get_time();
00340 while(unlocked.size()>0) {
00341 for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); ) {
00342 MotionCommand* mc=checkoutMotion(*it,false);
00343 if(mc==NULL)
00344 it++;
00345 else {
00346
00347 cur_cmd=*it;
00348 bool prune=true;
00349 try {
00350 prune=mc->shouldPrune();
00351 } catch(const std::exception& ex) {
00352 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand prune test, will prune",&ex);
00353 } catch(...) {
00354 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand prune test, will prune",NULL);
00355 }
00356 if(prune) {
00357 cout << "Removing expired " << *it << " (autoprune)" << endl;
00358 checkinMotion(*it);
00359
00360
00361
00362 removeMotion(*it);
00363 } else {
00364 try {
00365 mc->updateOutputs();
00366 } catch(const std::exception& ex) {
00367 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand updateOutputs",&ex);
00368 } catch(...) {
00369 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand updateOutputs",NULL);
00370 }
00371 checkinMotion(*it);
00372 }
00373 cur_cmd=invalid_MC_ID;
00374
00375 std::list<MC_ID>::iterator rem=it++;
00376 unlocked.erase(rem);
00377 lastProcessed=get_time();
00378 }
00379 }
00380 if(get_time()-lastProcessed>FrameTime*NumFrames/2)
00381 break;
00382 }
00383 if(unlocked.size()>0) {
00384 cerr << "Warning: MotionManager was unable to obtain a lock on MCs: ";
00385 for(std::list<MC_ID>::iterator it=unlocked.begin(); it!=unlocked.end(); it++)
00386 cerr << *it << ' ';
00387 cerr << endl;
00388 cerr << (unlocked.size()>1?"They":"It") << " may have been left locked by a Behavior while it was busy." << endl;
00389 cerr << "Try reducing the scope of your MMAccessor or call checkinMotion sooner." << endl;
00390 }
00391
00392
00393
00394 for(uint output=0; output<NumOutputs; output++) {
00395 cmdstatelist_t& curstatelist=cmdstates[output];
00396 for(cmdstatelist_t::index_t bit=curstatelist.begin(); bit!=curstatelist.end(); bit=curstatelist.next(bit)) {
00397 MC_ID high_ent=bit;
00398 float high_p=cmdlist[curstatelist[high_ent].mcid].priority;
00399 for(cmdstatelist_t::index_t cit=curstatelist.next(bit); cit!=curstatelist.end(); cit=curstatelist.next(cit)) {
00400 float curp=cmdlist[curstatelist[cit].mcid].priority;
00401 if(curp>high_p) {
00402 high_p=curp;
00403 high_ent=cit;
00404 }
00405 }
00406 curstatelist.swap(bit,high_ent);
00407
00408
00409
00410
00411 bit=high_ent;
00412 }
00413 }
00414
00415
00416
00417 for(uint frame=0; frame<NumFrames; frame++) {
00418 for(uint output=0; output<NumOutputs; output++) {
00419 cmdstatelist_t& curstatelist=cmdstates[output];
00420 float alpha=1;
00421 OutputCmd sumcmd;
00422
00423
00424
00425
00426
00427
00428 cmdstatelist_t::index_t ent=curstatelist.begin();
00429 while(ent!=curstatelist.end() && alpha>0) {
00430 OutputCmd curcmd;
00431 float curp=curstatelist[ent].priority;
00432 float curalpha=1;
00433 for(;ent!=curstatelist.end() && curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00434
00435 float curweight=curstatelist[ent].frames[frame].weight;
00436 ASSERT(curweight>=0,"negative output weights are illegal, MC_ID="<<curstatelist[ent].mcid<<" joint="<<outputNames[output]<<" frame="<<frame<<" weight="<<curweight);
00437 if(curweight<0)
00438 curweight=0;
00439 curcmd.value+=curstatelist[ent].frames[frame].value*curweight;
00440 curcmd.weight+=curweight;
00441 if(curweight<1)
00442 curalpha*=(1-curweight);
00443 else
00444 curalpha=0;
00445 }
00446 if(curcmd.weight>0) {
00447
00448 sumcmd.value+=curcmd.value/curcmd.weight*alpha*(1-curalpha);
00449 sumcmd.weight+=alpha*(1-curalpha);
00450 alpha*=curalpha;
00451 }
00452 }
00453
00454
00455
00456 if(sumcmd.weight>0) {
00457 sumcmd.value/=sumcmd.weight;
00458 outputs[frame][output]=sumcmd.value;
00459 } else
00460 sumcmd.value=outputs[frame][output]=cmdSums[output];
00461 if(frame==NumFrames-1)
00462 cmds[output]=sumcmd;
00463 }
00464 }
00465
00466 for(uint output=0; output<NumOutputs; output++)
00467 cmdSums[output]=outputs[NumFrames-1][output];
00468
00469 for (uint frame_idx = 0; frame_idx < NumFrames; frame_idx++) {
00470 for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++)
00471 outputs[frame_idx][output] = (outputs[frame_idx][output] + config->motion.calibration_offset[output-PIDJointOffset])
00472 * config->motion.calibration_scale[output-PIDJointOffset];
00473 }
00474
00475
00476
00477 for(uint output=PIDJointOffset; output<PIDJointOffset+NumPIDJoints; output++) {
00478 cmdstatelist_t& curstatelist=cmdstates[output];
00479 float alpha=1;
00480 float sumpid[3];
00481 for(uint i=0; i<3; i++)
00482 sumpid[i]=0;
00483 float sumweight=0;
00484 cmdstatelist_t::index_t ent=curstatelist.begin();
00485 while(ent!=curstatelist.end() && alpha>0) {
00486 float tmppid[3];
00487 for(uint i=0; i<3; i++)
00488 tmppid[i]=0;
00489 float tmpweight=0;
00490 float curp=curstatelist[ent].priority;
00491 float curalpha=1;
00492 for(;ent!=curstatelist.end() && curstatelist[ent].priority==curp; ent=curstatelist.next(ent)) {
00493
00494 float curweight=curstatelist[ent].pid.weight;
00495 ASSERT(curweight>=0,"negative PID weights are illegal")
00496 if(curweight<0)
00497 curweight=0;
00498 for(uint i=0; i<3; i++)
00499 tmppid[i]+=curstatelist[ent].pid.pid[i]*curweight;
00500 tmpweight+=curweight;
00501 if(curweight<1)
00502 curalpha*=(1-curweight);
00503 else
00504 curalpha=0;
00505 }
00506 if(tmpweight>0) {
00507
00508 for(uint i=0; i<3; i++)
00509 sumpid[i]+=tmppid[i]/tmpweight*(1-curalpha);
00510 sumweight+=(1-curalpha);
00511 alpha*=curalpha;
00512 }
00513 }
00514 if(sumweight>0) {
00515 for(uint i=0; i<3; i++)
00516 sumpid[i]/=sumweight;
00517 setPID(output,sumpid);
00518 }
00519 }
00520
00521 func_end();
00522
00523
00524 }
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552 bool
00553 #ifdef PLATFORM_APERIOS
00554 MotionManager::updatePIDs(OPrimitiveID primIDs[NumOutputs]) {
00555 #else
00556 MotionManager::updatePIDs() {
00557 #endif
00558
00559 bool dirty=!pidchanges.empty();
00560 while(!pidchanges.empty()) {
00561 #ifdef PLATFORM_APERIOS
00562 float gain[3];
00563 word shift[3];
00564
00565
00566 for(uint i=0; i<3; i++) {
00567 shift[i]=DefaultPIDShifts[i];
00568 gain[i]=pidchanges.front().pids[i]*(1<<(0x10-shift[i]));
00569 }
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596 OPENR::SetJointGain(primIDs[pidchanges.front().joint],(word)gain[0],(word)gain[1],(word)gain[2],shift[0],shift[1],shift[2]);
00597 #endif //PLATFORM_APERIOS or PLATFORM_LOCAL
00598 for(uint i=0; i<3; i++)
00599 state->pids[pidchanges.front().joint][i]=pidchanges.front().pids[i];
00600 pidchanges.pop_front();
00601 }
00602 return dirty;
00603 }
00604
00605 #ifdef PLATFORM_APERIOS
00606 void
00607 MotionManager::receivedMsg(const ONotifyEvent& event) {
00608
00609 func_begin();
00610
00611
00612 for(int x=0; x<event.NumOfData(); x++)
00613 processMsg(event.RCData(x));
00614
00615 func_end();
00616 }
00617 #else //PLATFORM_LOCAL
00618 bool
00619 MotionManager::receivedMsg(RCRegion* msg) {
00620 try {
00621 MarkScope l(*motman->procLocks[getAccID()]);
00622
00623
00624
00625 motman->processMsg(msg);
00626 } catch(const std::exception& ex) {
00627 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManagerMsg processing (MotionManager::receivedMsg)",&ex))
00628 throw;
00629 } catch(...) {
00630 if(!ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionManagerMsg processing (MotionManager::receivedMsg)",NULL))
00631 throw;
00632 }
00633 return true;
00634 }
00635 #endif //PLATFORM_APERIOS or PLATFORM_LOCAL
00636
00637 void
00638 MotionManager::processMsg(RCRegion * rcr) {
00639
00640 if(rcr==NULL) {
00641 cout << "WARNING: MotionManager::processMsg was given a NULL region" << endl;
00642 return;
00643 }
00644 func_begin();
00645
00646 MotionManagerMsg * mminfo = reinterpret_cast<MotionManagerMsg*>(rcr->Base());
00647 MC_ID mc_id=mminfo->mc_id;
00648 if(mc_id==invalid_MC_ID) {
00649
00650 func_end();
00651 return;
00652 }
00653 switch(mminfo->type) {
00654 case MotionManagerMsg::addMotion: {
00655 if(cmdlist[mc_id].rcr[getAccID()]==NULL) {
00656
00657 cmdlist[mc_id].rcr[getAccID()]=rcr;
00658
00659 rcr->AddReference();
00660
00661
00662
00663 cmdlist[mc_id].baseaddrs[getAccID()]=static_cast<MotionCommand*>(mminfo);
00664 } else {
00665 #ifdef PLATFORM_APERIOS
00666
00667 cerr << "WARNING: MotionManager::processMsg addMotion for motion which was already added!" << endl;
00668 #else
00669 ASSERT(cmdlist[mc_id].lastAccessor==(accID_t)-1 || cmdlist[mc_id].baseaddrs[getAccID()]==static_cast<MotionCommand*>(mminfo),"Baseaddr changed!");
00670 #endif
00671 }
00672 if(ProcessID::getID()==ProcessID::MotionProcess && cmdlist[mc_id].lastAccessor!=(accID_t)-1) {
00673 checkoutMotion(mc_id,true);
00674 try {
00675 cmdlist[mc_id].baseaddrs[getAccID()]->DoStart();
00676 } catch(const std::exception& ex) {
00677 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStart()",&ex);
00678 } catch(...) {
00679 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStart()",NULL);
00680 }
00681 checkinMotion(mc_id);
00682 }
00683 } break;
00684 case MotionManagerMsg::deleteMotion: {
00685
00686
00687 ASSERT(cmdlist[mc_id].lastAccessor==(accID_t)-1,"delete motion message for motion command not marked for deletion");
00688 if(cmdlist[mc_id].rcr[getAccID()]==NULL) {
00689 #ifndef PLATFORM_APERIOS
00690
00691 if(mminfo->creatorPID!=ProcessID::getID()) {
00692 #endif
00693 cout << "WARNING: MotionManager attempted to delete a NULL motion! mc_id="<<mc_id<<" process=" << ProcessID::getIDStr() << endl;
00694 stacktrace::displayCurrentStackTrace();
00695 #ifndef PLATFORM_APERIOS
00696 }
00697 #endif
00698 } else {
00699 cmdlist[mc_id].lock.lock(getAccID());
00700 int found=0;
00701 for(unsigned int i=0; i<numAcc; i++) {
00702 if(cmdlist[mc_id].rcr[i]!=NULL) {
00703 found++;
00704 }
00705 }
00706 if(ProcessID::getID()==ProcessID::MotionProcess) {
00707
00708 try {
00709 convertMotion(mc_id)->DoStop();
00710 } catch(const std::exception& ex) {
00711 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",&ex);
00712 } catch(...) {
00713 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",NULL);
00714 }
00715 cmdlist[mc_id].lastAccessor=(accID_t)-1;
00716 }
00717 cmdlist[mc_id].rcr[getAccID()]->RemoveReference();
00718 cmdlist[mc_id].rcr[getAccID()]=NULL;
00719 cmdlist[mc_id].baseaddrs[getAccID()]=NULL;
00720 if(found==1) {
00721 push_free(mc_id);
00722 } else {
00723 cmdlist[mc_id].lock.unlock();
00724 }
00725 if(ProcessID::getID()==ProcessID::MainProcess) {
00726 try {
00727 erouter->postEvent(EventBase::motmanEGID,mc_id,EventBase::deactivateETID,0);
00728 } catch(const std::exception& ex) {
00729 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",&ex);
00730 } catch(...) {
00731 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",NULL);
00732 }
00733 }
00734 }
00735
00736 } break;
00737 default:
00738 printf("*** WARNING *** unknown MotionManager msg type received\n");
00739 }
00740
00741 func_end();
00742 }
00743
00744
00745
00746
00747
00748
00749 MotionManager::MC_ID
00750 MotionManager::doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority) {
00751 MotionCommand * mc = dynamic_cast<MotionCommand*>(static_cast<MotionManagerMsg*>(sm.data()));
00752 if(mc==NULL) {
00753 cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl;
00754 return invalid_MC_ID;
00755 }
00756 mc->setAutoPrune(autoprune);
00757
00758 while(numAcc<MAX_ACCESS-1) { std::cout << "WAIT" << std::flush; }
00759 func_begin();
00760
00761
00762 for(MC_ID it=cmdlist.begin(); it!=cmdlist.end(); it=cmdlist.next(it)) {
00763 if(cmdlist[it].baseaddrs[getAccID()]==mc) {
00764 cerr << "Warning: adding motion command at " << mc << ", is already running in motion manager as MC_ID " << it << endl;
00765 return func_end(it);
00766 }
00767 }
00768 MC_ID mc_id = pop_free();
00769
00770 if(mc_id==cmdlist.end()) {
00771 cout << "MotionManager::addMotion() - Out of room, could not add" << endl;
00772 return func_end(cmdlist.end());
00773 }
00774
00775 mc->setAdd(mc_id);
00776 cmdlist[mc_id].baseaddrs[getAccID()]=mc;
00777 cmdlist[mc_id].rcr[getAccID()]=sm.getRegion();
00778
00779 cmdlist[mc_id].rcr[getAccID()]->AddReference();
00780
00781 cmdlist[mc_id].lastAccessor=getAccID();
00782 cmdlist[mc_id].priority=priority;
00783 try {
00784 erouter->postEvent(EventBase::motmanEGID,mc_id,EventBase::activateETID,0);
00785 } catch(const std::exception& ex) {
00786 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",&ex);
00787 } catch(...) {
00788 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",NULL);
00789 }
00790
00791 #ifdef PLATFORM_APERIOS
00792 OStatus err;
00793
00794
00795
00796
00797
00798
00799
00800
00801 err=subjs[getAccID()]->SetData(sm.getRegion());
00802 ASSERT(err==oSUCCESS,"*** ERROR MotionManager: SetData returned " << err);
00803
00804 err=subjs[getAccID()]->NotifyObservers();
00805 ASSERT(err==oSUCCESS,"*** ERROR MotionManager: NotifyObservers returned " << err);
00806 #else //PLATFORM_LOCAL
00807 try {
00808 subjs[getAccID()]->sendMessage(sm.getRegion());
00809 } catch(const std::exception& ex) {
00810 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",&ex);
00811 } catch(...) {
00812 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",NULL);
00813 }
00814 #endif //PLATFORM check for IPC stuff
00815
00816
00817 return func_end(mc_id);
00818 }
00819
00820 MotionCommand *
00821 MotionManager::checkoutMotion(MC_ID mcid,bool block) {
00822
00823 if(mcid>=MAX_MOTIONS || mcid==invalid_MC_ID) {
00824 cout << "*** ERROR *** " << ProcessID::getIDStr() << " tried to access invalid mcid " << mcid << endl;
00825 stacktrace::displayCurrentStackTrace();
00826 return NULL;
00827 }
00828 if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00829 cout << "*** WARNING *** " << ProcessID::getIDStr() << " tried to access dead mcid " << mcid << endl;
00830 stacktrace::displayCurrentStackTrace();
00831 return NULL;
00832 }
00833
00834 if(block)
00835 cmdlist[mcid].lock.lock(getAccID());
00836 else
00837 if(!cmdlist[mcid].lock.try_lock(getAccID()))
00838 return NULL;
00839 if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00840 cout << "*** WARNING *** " << ProcessID::getIDStr() << " tried to access mcid as it was removed " << mcid << endl;
00841 stacktrace::displayCurrentStackTrace();
00842 cmdlist[mcid].lock.unlock();
00843 return NULL;
00844 }
00845
00846
00847 return convertMotion(mcid);
00848 }
00849
00850 MotionCommand *
00851 MotionManager::convertMotion(MC_ID mc) {
00852 MotionCommand * base = cmdlist[mc].baseaddrs[getAccID()];
00853
00854 if(cmdlist[mc].lastAccessor!=getAccID()) {
00855
00856
00857
00858
00859 cmdlist[mc].lastAccessor=getAccID();
00860 }
00861 base->setTranslator(etrans);
00862 #ifdef PLATFORM_APERIOS
00863 base->setWorldState(state);
00864 #endif
00865 return base;
00866 }
00867
00868 void
00869 MotionManager::checkinMotion(MC_ID mcid) {
00870 if(mcid>=MAX_MOTIONS || mcid==invalid_MC_ID)
00871 return;
00872 if(cmdlist[mcid].lock.get_lock_level()==1 && cmdlist[mcid].rcr[getAccID()]!=NULL) {
00873 MotionCommand * base = cmdlist[mcid].baseaddrs[getAccID()];
00874 base->setTranslator(NULL);
00875 #ifdef PLATFORM_APERIOS
00876 base->setWorldState(NULL);
00877 #endif
00878 }
00879 cmdlist[mcid].lock.unlock();
00880 }
00881
00882 void
00883 MotionManager::removeMotion(MC_ID mcid) {
00884 if(mcid>=MAX_MOTIONS || mcid==invalid_MC_ID)
00885 return;
00886 func_begin();
00887 if(cmdlist[mcid].lastAccessor==(accID_t)-1) {
00888 cout << "WARNING: removeMotion called for a motion which has already been removed mc_id="<<mcid<<" process=" << ProcessID::getIDStr() << endl;
00889 stacktrace::displayCurrentStackTrace();
00890 func_end();
00891 return;
00892 }
00893 if(cmdlist[mcid].rcr[getAccID()]==NULL) {
00894 cout << "WARNING: removeMotion called for a NULL motion! mc_id="<<mcid<<" process=" << ProcessID::getIDStr() << endl;
00895 stacktrace::displayCurrentStackTrace();
00896 func_end();
00897 return;
00898 }
00899 cmdlist[mcid].lock.lock(getAccID());
00900 if(ProcessID::getID()==ProcessID::MotionProcess) {
00901 MotionCommand * mc=checkoutMotion(mcid,true);
00902 try {
00903 mc->DoStop();
00904 } catch(const std::exception& ex) {
00905 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",&ex);
00906 } catch(...) {
00907 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during MotionCommand DoStop()",NULL);
00908 }
00909 checkinMotion(mcid);
00910 }
00911 cmdlist[mcid].lastAccessor=(accID_t)-1;
00912 cmdlist[mcid].rcr[getAccID()]->RemoveReference();
00913 cmdlist[mcid].rcr[getAccID()]=NULL;
00914 cmdlist[mcid].baseaddrs[getAccID()]=NULL;
00915 cmdlist[mcid].lock.unlock();
00916 if(ProcessID::getID()==ProcessID::MainProcess) {
00917 try {
00918 erouter->postEvent(EventBase::motmanEGID,mcid,EventBase::deactivateETID,0);
00919 } catch(const std::exception& ex) {
00920 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",&ex);
00921 } catch(...) {
00922 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during event posting",NULL);
00923 }
00924 }
00925 #ifdef PLATFORM_APERIOS
00926 MotionManagerMsg dmsg;
00927 dmsg.setDelete(mcid);
00928
00929 subjs[getAccID()]->SetData(&dmsg,sizeof(dmsg));
00930 subjs[getAccID()]->NotifyObservers();
00931 #else //PLATFORM_LOCAL
00932
00933 SharedObject<MotionManagerMsg> dmsg;
00934 dmsg->setDelete(mcid);
00935
00936 try {
00937 subjs[getAccID()]->sendMessage(dmsg.getRegion());
00938 } catch(const std::exception& ex) {
00939 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",&ex);
00940 } catch(...) {
00941 ProjectInterface::uncaughtException(__FILE__,__LINE__,"Occurred during message sending",NULL);
00942 }
00943 #endif //PLATFORM check for IPC stuff
00944 func_end();
00945 }
00946
00947
00948
00949
00950
00951
00952 void
00953 MotionManager::setPID(unsigned int joint, const float pids[3]) {
00954 func_begin();
00955
00956
00957 for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) {
00958 if(pidchanges[u].joint==joint) {
00959 for(uint i=0; i<3; i++) {
00960 pidchanges[u].pids[i]=pids[i];
00961 if(pids[i]!=state->pids[joint][i]) {
00962 for(i++; i<3; i++)
00963 pidchanges[u].pids[i]=pids[i];
00964 func_end();
00965 return;
00966 }
00967 }
00968
00969
00970 pidchanges.erase(u);
00971 func_end();
00972 return;
00973 }
00974 }
00975
00976
00977 for(uint i=0; i<3; i++)
00978 if(pids[i]!=state->pids[joint][i]) {
00979 PIDUpdate update(joint,pids);
00980 pidchanges.push_back(update);
00981
00982
00983
00984 break;
00985 }
00986 func_end();
00987 }
00988
00989
00990 MotionManager::MC_ID
00991 MotionManager::skip_ahead(MC_ID mcid) const {
00992
00993
00994 while(mcid!=cmdlist.end() && cmdlist[mcid].rcr[getAccID()]==NULL)
00995 mcid=cmdlist.next(mcid);
00996 return mcid;
00997 }
00998
00999 MotionManager::OutputState::OutputState()
01000 : priority(0),mcid(MotionManager::invalid_MC_ID), pid()
01001 {}
01002 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames])
01003 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
01004 {
01005 for(unsigned int i=0; i<NumFrames; i++)
01006 frames[i]=cmds[i];
01007 }
01008 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd)
01009 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
01010 {
01011 for(unsigned int i=0; i<NumFrames; i++)
01012 frames[i]=cmd;
01013 }
01014 MotionManager::OutputState::OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame)
01015 : priority(pri),mcid(mc), pid(DefaultPIDs[out])
01016 {
01017 frames[frame]=cmd;
01018 }
01019 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputPID& p)
01020 : priority(pri),mcid(mc), pid(p)
01021 {}
01022 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p)
01023 : priority(pri),mcid(mc), pid(p)
01024 {
01025 for(unsigned int i=0; i<NumFrames; i++)
01026 frames[i]=cmds[i];
01027 }
01028 MotionManager::OutputState::OutputState(unsigned int , float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p)
01029 : priority(pri),mcid(mc), pid(p)
01030 {
01031 for(unsigned int i=0; i<NumFrames; i++)
01032 frames[i]=cmd;
01033 }
01034
01035
01036
01037
01038
01039
01040
01041
01042
01043
01044
01045
01046
01047
01048
01049
01050
01051
01052
01053
01054
01055
01056
01057
01058
01059
01060
01061
01062
01063
01064
01065
01066
01067
01068
01069
01070
01071
01072
01073
01074
01075
01076
01077
01078
01079
01080
01081
01082
01083
01084
01085
01086