Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
MotionManager.hGo to the documentation of this file.00001 //-*-c++-*- 00002 #ifndef INCLUDED_MotionManager_h 00003 #define INCLUDED_MotionManager_h 00004 00005 #include "OutputCmd.h" 00006 #include "OutputPID.h" 00007 #include "Shared/RobotInfo.h" 00008 #include "Shared/StackTrace.h" 00009 #include "IPC/ListMemBuf.h" 00010 #include "IPC/MutexLock.h" 00011 #include "MotionManagerMsg.h" 00012 00013 #ifdef PLATFORM_APERIOS 00014 # include <OPENR/OPENR.h> 00015 # include <OPENR/OPENRAPI.h> 00016 # include <OPENR/OSubject.h> 00017 # include <OPENR/ObjcommEvent.h> 00018 # include <OPENR/OObject.h> 00019 #else //PLATFORM_LOCAL 00020 class MessageQueueBase; 00021 class MessageReceiver; 00022 #endif 00023 00024 class EventTranslator; 00025 class MotionCommand; 00026 class RCRegion; 00027 class SharedObjectBase; 00028 00029 //! The purpose of this class is to repeatedly compute the final set of joint angles for the robot, managing a set of (possibly conflicting) MotionCommands 00030 /*! Since Main and Motion run as separate processes, they 00031 * could potentially try to access the same motion command at the 00032 * same time, leading to unpredictable behavior. The MotionManager 00033 * enforces a set of locks to serialize access to the MotionCommands. 00034 * Although you could call checkoutMotion() and checkinMotion() directly, 00035 * it is instead recommended to use MMAccessor to automatically handle 00036 * casting and checkin for you. 00037 * 00038 * The other problem is that we are sharing the memory holding MotionCommands between processes. 00039 * MotionManager will do the necessary magic behind the scenes to distribute new 00040 * MotionCommands to all the involved processes (currently just Main and 00041 * Motion)\n You can create and add a new motion in one line: 00042 * 00043 * @code 00044 * // type A: prunable motions are automatically removed when completed: 00045 * motman->addPrunableMotion( SharedObject<YourMC>([arg1,...]) [, priority ] ); 00046 * 00047 * // type B: persistent motions are removed only when you explicitly request it: 00048 * MC_ID id = motman->addPersistentMotion( SharedObject<YourMC>([arg1,...]) [, priority ] ); 00049 * // then later: motman->removeMotion(id); 00050 * @endcode 00051 * 00052 * The priority level can be changed later via setPriority(), and there are 00053 * symbolic values defined in #kIgnoredPriority through #kEmergencyPriority 00054 * to give some common guidelines on the magnitudes to use. The default 00055 * priority level if unspecified is #kStdPriority. 00056 * 00057 * If you want to do some more initializations not handled by the MotionCommand's 00058 * constructor (the @p arg1, @p arg2, ... params) then you would 00059 * want to do something like the following: 00060 * 00061 * @code 00062 * SharedObject<YourMC> yourmc([arg1,[arg2,...]]); 00063 * yourmc->cmd1(); 00064 * yourmc->cmd2(); 00065 * //... 00066 * motman->addPrunableMotion(yourmc [, ...]); //or addPersistentMotion(...) 00067 * @endcode 00068 * 00069 * Notice that @c yourmc is actually of type SharedObject, but you're calling @c 00070 * YourMC's functions on it through the '->' operator... SharedObject is a "smart pointer" which 00071 * will pass your function calls on to the underlying templated type. 00072 * Isn't C++ great? :) 00073 * 00074 * @warning Once the MotionCommand has been added, you must check it 00075 * out to make any future modifications it or risk concurrent access problems. 00076 * In other words, you should @e not keep the SharedObject and continue 00077 * to access the motion through that unless you know the motion is not active 00078 * in the MotionManager. Instead, always use a MMAccessor. 00079 * 00080 * @see MMAccessor for information on accessing motions after you've 00081 * added them to MotionManager, or if it @e may be active in the MotionManager. 00082 * 00083 * @see MotionCommand for information on creating new motion primitives. 00084 */ 00085 class MotionManager { 00086 public: 00087 //! This is the number of processes which will be accessing the MotionManager 00088 /*! Probably just MainObject and MotionObject... This isn't really a 00089 * hard maximum, but should be actual expected, need to know when 00090 * they're all connected */ 00091 static const unsigned int MAX_ACCESS=2; 00092 00093 static const unsigned int MAX_MOTIONS=64; //!< This is the maximum number of Motions which can be managed, can probably be increased reasonably without trouble 00094 00095 typedef MotionManagerMsg::MC_ID MC_ID; //!< use this type when referring to the ID numbers that MotionManager hands out 00096 static const MC_ID invalid_MC_ID=MotionManagerMsg::invalid_MC_ID; //!< for errors and undefined stuff 00097 00098 //!Just to give you some guidelines for what values to use for different priority levels, but you can pick any value you like (that's why they are floats) 00099 //!@name Priority Level Constants 00100 static const float kIgnoredPriority; //!< won't be expressed, handy if you want to temporarily pause something 00101 static const float kBackgroundPriority; //!< will only be expressed if *nothing* else is using that joint 00102 static const float kLowPriority; //!< for stuff that's not background but lower than standard 00103 static const float kStdPriority; //!< for every-day commands 00104 static const float kHighPriority; //!< for stuff that should override standard stuff 00105 static const float kEmergencyPriority; //!< for really important stuff, such as the emergency stop 00106 //@} 00107 00108 MotionManager(); //!< Constructor, sets all the outputs to 0 00109 #ifdef PLATFORM_APERIOS 00110 void InitAccess(OSubject* subj); //!< @b LOCKS @b MotionManager Everyone who is planning to use the MotionManager needs to call this before they access it or suffer a horrible fate 00111 void receivedMsg(const ONotifyEvent& event); //!< @b LOCKS @b MotionManager This gets called by an OObject when it receives a message from one of the other OObject's MotionManagerComm Subject 00112 #else 00113 void InitAccess(MessageQueueBase& mcbufq, Resource& behaviorLock); //!< @b LOCKS @b MotionManager Everyone who is planning to use the MotionManager needs to call this before they access it or suffer a horrible fate 00114 static bool receivedMsg(RCRegion* msg); //!< called with incoming messages, will pass non-echos to processMsg() 00115 #endif 00116 void RemoveAccess(); //!< needed in order to dereference shared memory regions before shutting down 00117 static void setTranslator(EventTranslator* et) {etrans=et;} //!< sets #etrans, should be called before any events can be sent 00118 void processMsg(RCRegion* region); //!< @b LOCKS @b MotionManager This gets called by receivedMsg when under Aperios, or directly if you already have an RCRegion 00119 ~MotionManager(); //!<destructor 00120 void motionReport() const; //!< displays a report of active motion commands on stderr 00121 00122 //!@name MotionCommand Safe 00123 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd); //!< @b LOCKS @b MotionManager Requests a value be set for the specified output, copies cmd across frames 00124 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, unsigned int frame); //!< @b LOCKS @b MotionManager Requests a value be set for the specified output in the specified frame 00125 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd cmd[NumFrames]); //!< @b LOCKS @b MotionManager Requests a value be set for the specified output across frames 00126 void setOutput(const MotionCommand* caller, unsigned int output, const OutputPID& pid); //!< @b LOCKS @b MotionManager Requests a PID be set for the specified output, notice that this might be overruled by a higher priority motion 00127 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd& cmd, const OutputPID& pid); //!< @b LOCKS @b MotionManager Requests a value and PID be set for the specified output 00128 void setOutput(const MotionCommand* caller, unsigned int output, const OutputCmd cmd[NumFrames], const OutputPID& pid); //!< @b LOCKS @b MotionManager Requests a value and PID be set for the specified output 00129 #ifndef TGT_DYNAMIC 00130 const OutputCmd& getOutputCmd(unsigned int output) const { return cmds[output]; } //!< Returns the value of the output last sent to the OS. Note that this will differ from the sensed value in state, even when staying still. There is no corresponding getOutputPID because this value *will* duplicate the value in state. 00131 #endif 00132 void setPriority(MC_ID mcid, float p) { if ( mcid != invalid_MC_ID ) cmdlist[mcid].priority=p; }//!< sets the priority level of a MotionCommand, symbolic values are available to give some guidelines -- see #kIgnoredPriority through #kEmergencyPriority 00133 float getPriority(MC_ID mcid) const { return cmdlist[mcid].priority; } //!< returns priority level of a MotionCommand, symbolic values are available to give some guidelines -- see #kIgnoredPriority through #kEmergencyPriority 00134 //@} 00135 00136 //@{ 00137 inline MC_ID begin() const { return skip_ahead(cmdlist.begin()); } //!< returns the MC_ID of the first MotionCommand 00138 inline MC_ID next(MC_ID cur) const { return skip_ahead(cmdlist.next(cur)); } //!< returns the MC_ID of MotionCommand following the one that is passed 00139 inline MC_ID end() const { return cmdlist.end(); } //!< returns the MC_ID of the one-past-the-end MotionCommand (like the STL) 00140 inline unsigned int size() const { return cmdlist.size(); } //!< returns the number of MotionCommands being managed 00141 //@} 00142 00143 //!You can have one MC check out and modify another, but make sure the other MC doesn't call setOutput() 00144 //!@name MotionCommand "Risky" 00145 MotionCommand * checkoutMotion(MC_ID mcid,bool block=true) const; //!< locks the command and possibly performs RTTI conversion; supports recursive calls 00146 void checkinMotion(MC_ID mcid) const; //!< marks a MotionCommand as unused 00147 MotionCommand * peekMotion(MC_ID mcid) const { return mcid==invalid_MC_ID?NULL:cmdlist[mcid].baseaddrs[getAccID()]; } //!< allows access to a MotionCommand without checking it out; warning @b never call a function based on this, only access member fields through it 00148 unsigned int checkoutLevel(MC_ID mcid) const { return mcid==invalid_MC_ID?0:cmdlist[mcid].lock.get_lock_level(); } //!< returns the number of times @a mcid has been checked out minus the times it's been checked in 00149 bool isOwner(MC_ID mcid) const { return mcid==invalid_MC_ID?false:(cmdlist[mcid].lock.owner()==getAccID()); } 00150 //@} 00151 00152 //!@name MotionCommand Unsafe 00153 //@{ 00154 //! @b LOCKS @b MotionManager adds a new motion (wrapped in a SharedObject) and marks that it should be automatically deleted when the MotionCommand::isAlive() returns false. 00155 MC_ID addPrunableMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,true,priority); } 00156 //! @b LOCKS @b MotionManager adds a new motion (wrapped in a SharedObject) and marks that it should @e not be deleted, until removeMotion(MC_ID mcid) is called. 00157 MC_ID addPersistentMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,false,priority); } 00158 void removeMotion(MC_ID mcid); //!< @b LOCKS @b MotionManager removes the specified MotionCommand 00159 //@} 00160 00161 //@{ 00162 void lock() { MMlock.lock(getAccID()); } //!< gets an exclusive lock on MotionManager - functions marked @b LOCKS @b MotionManager will cause (and require) this to happen automatically 00163 bool trylock() { return MMlock.try_lock(getAccID()); } //!< tries to get a lock without blocking 00164 void unlock() { MMlock.unlock(); } //!< releases a lock on the motion manager 00165 //@} 00166 00167 //@{ 00168 #ifndef TGT_DYNAMIC 00169 void getOutputs(float outputs[][NumOutputs]); //!< @b LOCKS @b MotionManager called by MotionObject to fill in the output values for the next ::NumFrames frames (only MotoObj should call this...) 00170 #endif 00171 #ifdef PLATFORM_APERIOS 00172 bool updatePIDs(OPrimitiveID primIDs[NumOutputs]); //!< call this when you want MotionManager to update modified PID values, returns true if changes made (only MotoObj should be calling this...), see PIDMC for general PID documentation 00173 #else 00174 bool updatePIDs(std::vector<std::pair<unsigned int, float[3]> >& pids); //!< call this when you want MotionManager to update modified PID values, returns true if changes made (only MotoObj should be calling this...), see PIDMC for general PID documentation 00175 #endif 00176 //@} 00177 00178 //! holds the full requested value of an output 00179 class OutputState { 00180 public: 00181 //!@name Constructors 00182 //!Constructor 00183 OutputState(); 00184 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames]); 00185 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd); 00186 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame); 00187 OutputState(unsigned int out, float pri, MC_ID mc, const OutputPID& p); 00188 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p); 00189 OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p); 00190 //@} 00191 float priority; //!< priority level 00192 MC_ID mcid; //!< MC_ID of requester 00193 OutputCmd frames[NumFrames]; //!< values of output planned ahead 00194 OutputPID pid; //!< pid of output 00195 }; 00196 00197 bool hasReference(ProcessID::ProcessID_t proc, MC_ID mcid) const { return cmdlist[mcid].rcr[_MMaccID[proc]]!=NULL; } 00198 00199 protected: 00200 //!does the actual work of adding a motion 00201 MC_ID doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority); 00202 //! sets up a motion command to be accessed by the current process 00203 MotionCommand* convertMotion(MC_ID mc) const; 00204 00205 //! used to request pids for a given joint 00206 struct PIDUpdate { 00207 //!constructor 00208 PIDUpdate() : joint((unsigned int)-1) {} 00209 //!constructor 00210 PIDUpdate(unsigned int j, const float p[3]) : joint(j) { 00211 for(unsigned int i=0; i<3; i++) 00212 pids[i]=p[i]; 00213 } 00214 unsigned int joint; //!< the joint ID (see RobotInfo.h for offset values) 00215 float pids[3]; //!< the PID values to use (see ::Pid ) 00216 }; 00217 #ifndef TGT_DYNAMIC 00218 ListMemBuf<PIDUpdate,NumPIDJoints> pidchanges; //!< stores PID updates, up to one per joint (if same is set more than once, it's just overwrites previous update) 00219 #endif 00220 void setPID(unsigned int j, const float p[3]); //!< @b LOCKS @b MotionManager, called internally to do the work of setting the PID... you probably want to call setOutput with an OutputPID argument, not this... 00221 00222 typedef unsigned short accID_t; //!< type to use to refer to accessors of MotionManager (or its locks) 00223 00224 void func_begin() const { MMlock.lock(getAccID()); } //!< called at the begining of many functions to lock MotionManager 00225 void func_end() const { MMlock.unlock(); } //!< called at the end of a function which called func_begin() to release it 00226 template<class T> T func_end(T val) const { func_end(); return val; } //!< same as func_end(), except passes return value through 00227 00228 MC_ID skip_ahead(MC_ID mcid) const; //!< during iteration, skips over motioncommands which are still in transit from on OObject to another 00229 00230 //!All the information we need to maintain about a MotionCommand 00231 struct CommandEntry { 00232 //! Constructor, sets everything to basics 00233 CommandEntry() : lastAccessor((unsigned short)-1),lock(),priority(MotionManager::kStdPriority), trace(stacktrace::recordStackTrace(10,5)) { 00234 for(unsigned int i=0; i<MAX_ACCESS; i++) { 00235 baseaddrs[i]=NULL; 00236 rcr[i]=NULL; 00237 } 00238 } 00239 ~CommandEntry() { stacktrace::freeStackTrace(trace); trace=NULL; } 00240 MotionCommand * baseaddrs[MAX_ACCESS]; //!< for each accessor, the base address of the motion command 00241 RCRegion * rcr[MAX_ACCESS]; //!< for each accessor the shared memory region that holds the motion command 00242 mutable accID_t lastAccessor; //!< the ID of the last accessor to touch the command (which implies if it wants to touch this again, we don't have to convert again) 00243 mutable MutexLock<MAX_ACCESS> lock; //!< a lock to maintain mutual exclusion 00244 float priority; //!< MotionCommand's priority level 00245 stacktrace::StackFrame* trace; //records the stack trace where the motion was added to the motion manager for leak tracking 00246 private: 00247 CommandEntry(const CommandEntry&); //!< this shouldn't be called... 00248 CommandEntry& operator=(const CommandEntry&); //!< this shouldn't be called... 00249 }; 00250 ListMemBuf<CommandEntry,MAX_MOTIONS,MC_ID> cmdlist; //!< the list where MotionCommands are stored, remember, we're in a shared memory region with different base addresses - no pointers! 00251 MC_ID cur_cmd; //!< MC_ID of the MotionCommand currently being updated by getOutputs(), or NULL if not in getOutputs. This is used by the setOutput()'s to tell which MotionCommand is calling 00252 00253 00254 inline MC_ID pop_free() { return cmdlist.new_front(); } //!<pulls an entry from cmdlist's free section and returns its index 00255 inline void push_free(MC_ID a) { cmdlist.erase(a); } //!<puts an entry back into cmdlist's free section 00256 00257 mutable MutexLock<MAX_ACCESS> MMlock; //!< The main lock for the class 00258 00259 typedef ListMemBuf<OutputState,MAX_MOTIONS> cmdstatelist_t; //!< shorthand for a list of OutputState's 00260 #ifndef TGT_DYNAMIC 00261 cmdstatelist_t cmdstates[NumOutputs]; //!< requested positions by each of the MC's for each of the outputs 00262 float cmdSums[NumOutputs]; //!<Holds the final values for the outputs of the last frame generated 00263 OutputCmd cmds[NumOutputs]; //!<Holds the weighted values and total weight for the outputs of the last frame 00264 #endif 00265 00266 accID_t numAcc; //!<The number of accessors who have registered with InitAccess() 00267 #ifdef PLATFORM_APERIOS 00268 OSubject* subjs[MAX_ACCESS]; //!<The OSubject for each process (accessor) on which it should be broadcast when a command is added 00269 #else //PLATFORM_LOCAL 00270 //!Storage of each process's attachment of the message queue, used to internally transmit sound buffers to SoundPlay 00271 MessageQueueBase * subjs[MAX_ACCESS]; 00272 MessageReceiver * mcrecvs[MAX_ACCESS]; //!< message receivers which watch for incoming motion command regions, or requests to free them 00273 Resource* procLocks[MAX_ACCESS]; //!< pointers to per-process thread locks, acquired during message processing from one of #mcrecvs 00274 #endif 00275 00276 static int getAccID() { return _MMaccID[ProcessID::getID()]; } 00277 static int _MMaccID[ProcessID::NumProcesses]; //!<Stores the accessor id assigned in InitAccess() for each process 00278 static EventTranslator* etrans; //!< EventTranslator for sending events to Main -- each process will set the correct value for calls within that process. 00279 00280 private: 00281 MotionManager(const MotionManager&); //!< this shouldn't be called... 00282 MotionManager& operator=(const MotionManager&); //!< this shouldn't be called... 00283 }; 00284 00285 //!anyone who includes MotionManager.h will be wanting to use the global motman... don't want multiple of these! created by MotoObj 00286 extern MotionManager * motman; 00287 00288 /*! @file 00289 * @brief Describes MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access 00290 * @author ejt (Creator) 00291 */ 00292 00293 #endif |
Tekkotsu v5.1CVS |
Generated Mon May 9 04:58:45 2016 by Doxygen 1.6.3 |