Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MotionManager.h

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

Tekkotsu v4.0
Generated Thu Nov 22 00:54:54 2007 by Doxygen 1.5.4