Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

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 "Shared/ListMemBuf.h"
00010 #include "Shared/MutexLock.h"
00011 
00012 #ifdef PLATFORM_APERIOS
00013 #include "Shared/SharedObject.h"
00014 #include "MotionManagerMsg.h"
00015 #endif
00016 
00017 #ifdef PLATFORM_APERIOS
00018 #include <OPENR/OPENR.h>
00019 #include <OPENR/OPENRAPI.h>
00020 #include <OPENR/OSubject.h>
00021 #include <OPENR/ObjcommEvent.h>
00022 #include <OPENR/OObject.h>
00023 #endif
00024 
00025 //! The purpose of this class is to serialize access to the MotionCommands and simplify their sharing between memory spaces
00026 /*! Since MotionObject and MainObject run as separate processes, they could potentially try to access the
00027  *  same motion command at the same time, leading to unpredictable behavior.  The MotionManager enforces
00028  *  a set of locks to solve this\n
00029  *  The other problem is that we are sharing between processes.  MotionManager will do what's necessary to distribute new
00030  *  MotionCommand's to all the processes (currently just MainObj and MotoObj)\n
00031  *  You should be able to create and add a new motion in one line:
00032  *  @code
00033  *  motman->addMotion(SharedObject<YourMC>([arg1,[arg2,...]]) [, priority [, autoprune] ]);
00034  *  @endcode
00035  *  But if you want to do some more initializations not handled by the constructor (the @p arg1, @p arg2, ...
00036  *  params) then you would want to do something like the following:
00037  *  @code
00038  *  SharedObject<YourMC> tmpvar([arg1,[arg2,...]]);
00039  *  tmpvar->cmd1();
00040  *  tmpvar->cmd2();
00041  *  //...
00042  *  motman->addMotion(tmpvar [, ...]);
00043  *  @endcode
00044  *
00045  *  Notice that tmpvar is of type SharedObject, but you're calling YourMC functions on it...
00046  *  SharedObject is a "smart pointer" which will pass your function calls on to the
00047  *  underlying templated type.  Isn't C++ great? :)
00048  *
00049  *  @warning Once the MotionCommand has been added you must check it
00050  *  out to modify it or risk concurrent access problems.
00051  *
00052  *  @see MotionCommand for information on creating new motion primitives.
00053  *
00054  *  @see MMAccessor for information on accessing motions after you've added them to MotionManager.
00055  */
00056 class MotionManager {
00057 public:
00058   //! This is the number of processes which will be accessing the MotionManager
00059   /*! Probably just MainObject and MotionObject
00060    *  Isn't really a hard maximum, but should be actual expected, need to know when they're all connected */
00061   static const unsigned int MAX_ACCESS=2;
00062 
00063   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
00064 
00065   typedef MotionManagerMsg::MC_ID MC_ID;      //!< use this type when referring to the ID numbers that MotionManager hands out
00066   static const MC_ID invalid_MC_ID=(MC_ID)-1; //!< for errors and undefined stuff
00067 
00068   MotionManager();                            //!< Constructor, sets all the outputs to 0
00069 
00070 #ifdef PLATFORM_APERIOS
00071   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
00072   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
00073 #endif
00074 
00075   //!@name MotionCommand Safe
00076   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
00077   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
00078   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
00079   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
00080   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
00081   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
00082   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.
00083   void setPriority(MC_ID mcid, float p) { cmdlist[mcid].priority=p; }//!< sets the priority level of a MotionCommand
00084   float getPriority(MC_ID mcid) const { return cmdlist[mcid].priority; } //!< returns priority level of a MotionCommand
00085   //@}
00086 
00087   //@{
00088   inline MC_ID begin() const         { return skip_ahead(cmdlist.begin()); }   //!< returns the MC_ID of the first MotionCommand
00089   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
00090   inline MC_ID end() const           { return cmdlist.end();      } //!< returns the MC_ID of the one-past-the-end MotionCommand (like the STL)
00091   inline unsigned int size() const   { return cmdlist.size();     } //!< returns the number of MotionCommands being managed
00092   //@}
00093 
00094   //!You can have one MC check out and modify another, but make sure the other MC doesn't call setOutput()
00095   //!@name MotionCommand "Risky"
00096   MotionCommand * checkoutMotion(MC_ID mcid,bool block=true); //!< locks the command and possibly performs RTTI conversion; supports recursive calls
00097   void checkinMotion(MC_ID mcid); //!< marks a MotionCommand as unused
00098   MotionCommand * peekMotion(MC_ID mcid) { return cmdlist[mcid].baseaddrs[_MMaccID]; } //!< allows access to a MotionCommand without checking it out @warning @b never call a function based on this, only access member fields through it
00099   unsigned int checkoutLevel(MC_ID mcid) { return 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
00100   //@}
00101 
00102   //!@name MotionCommand Unsafe
00103   //@{
00104 #ifdef PLATFORM_APERIOS
00105   MC_ID addMotion(const SharedObjectBase& sm); //!< @b LOCKS @b MotionManager Creates a new MotionCommand, automatically sharing it between processes (there is some lag time here)
00106   MC_ID addMotion(const SharedObjectBase& sm, float priority); //!< @b LOCKS @b MotionManager allows a quick way to set a priority level of a new MotionCommand
00107   MC_ID addMotion(const SharedObjectBase& sm, bool autoprune); //!< @b LOCKS @b MotionManager allows a quick was to set the autoprune flag
00108   MC_ID addMotion(const SharedObjectBase& sm, float priority, bool autoprune); //!< @b LOCKS @b MotionManager Call one of these to add a MotionCommand to the MotionManager, using the SharedObject class
00109 #endif //PLATFORM_APERIOS
00110   void removeMotion(MC_ID mcid); //!< @b LOCKS @b MotionManager removes the specified MotionCommand
00111   //@}
00112 
00113   //@{
00114   void lock()    { MMlock.lock(_MMaccID); } //!< gets an exclusive lock on MotionManager - functions marked @b LOCKS @b MotionManager will cause (and require) this to happen automatically
00115   bool trylock() { return MMlock.try_lock(_MMaccID); } //!< tries to get a lock without blocking
00116   void release() { MMlock.release(); } //!< releases a lock on the motion manager
00117   //@}
00118 
00119   //@{
00120   void getOutputs(float outputs[NumFrames][NumOutputs]);  //!< @b LOCKS @b MotionManager called by MotionObject to fill in the output values for the next ::NumFrames frames (only MotoObj should call this...)
00121   void updateWorldState();                                //!< call this when you want MotionManager to set the WorldState to reflect what things should be for unsensed outputs (LEDs, ears) (only MotoObj should be calling this...)
00122 #ifdef PLATFORM_APERIOS
00123   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...)
00124 #endif
00125   //@}
00126 
00127   //! holds the full requested value of an output
00128   class OutputState {
00129   public:
00130     //!@name Constructors
00131     //!Constructor
00132     OutputState();
00133     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames]);
00134     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd);
00135     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, unsigned int frame);
00136     OutputState(unsigned int out, float pri, MC_ID mc, const OutputPID& p);
00137     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd cmds[NumFrames], const OutputPID& p);
00138     OutputState(unsigned int out, float pri, MC_ID mc, const OutputCmd& cmd, const OutputPID& p);
00139     //@}
00140     float priority;             //!< priority level
00141     MC_ID mcid;                 //!< MC_ID of requester
00142     OutputCmd frames[NumFrames]; //!< values of output planned ahead
00143     OutputPID pid;               //!< pid of output
00144   };
00145 
00146   //!Just to give you a general idea what values to use for different priority levels
00147   //!@name Priority Level Constants
00148   static const float kIgnoredPriority;    //!< won't be expressed, handy if you want to temporarily pause something
00149   static const float kBackgroundPriority; //!< will only be expressed if *nothing* else is using that joint
00150   static const float kLowPriority;        //!< for stuff that's not background but lower than standard
00151   static const float kStdPriority;        //!< for every-day commands
00152   static const float kHighPriority;       //!< for stuff that should over ride standard stuff
00153   static const float kEmergencyPriority;  //!< for really important stuff, such as the emergency stop
00154 
00155 protected:
00156   //! used to request pids for a given joint
00157   struct PIDUpdate {
00158     //!constructor
00159     PIDUpdate() : joint((unsigned int)-1) {}
00160     //!constructor
00161     PIDUpdate(unsigned int j, const float p[3]) : joint(j) {
00162       for(unsigned int i=0; i<3; i++)
00163         pids[i]=p[i];
00164     }
00165     unsigned int joint; //!< the joint ID (see RobotInfo.h for offset values)
00166     float pids[3]; //!< the PID values to use (see ::Pid )
00167   };
00168   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)
00169   void setPID(unsigned int j, const float p[3]); //!< @b LOCKS @b MotionManager you can call this to set the PID values directly (instead of using a motion command) Be careful though
00170 
00171   typedef unsigned short accID_t; //!< type to use to refer to accessors of MotionManager (or its locks)
00172 
00173   void func_begin() { MMlock.lock(_MMaccID); } //!< called at the begining of many functions to lock MotionManager
00174   void func_end() { MMlock.release(); } //!< called at the end of a function which called func_begin() to release it
00175   template<class T> T func_end(T val) { func_end(); return val; } //!< same as func_end(), except passes return value through
00176 
00177   MC_ID skip_ahead(MC_ID mcid) const; //!< during iteration, skips over motioncommands which are still in transit from on OObject to another
00178     
00179   //!All the information we need to maintain about a MotionCommand
00180   struct CommandEntry {
00181     //! Constructor, sets everything to basics
00182     CommandEntry() : lastAccessor((unsigned short)-1),lock(),priority(MotionManager::kStdPriority) {
00183       for(unsigned int i=0; i<MAX_ACCESS; i++) {
00184         baseaddrs[i]=NULL;
00185 #ifdef PLATFORM_APERIOS
00186         rcr[i]=NULL;
00187 #endif
00188       }
00189     }
00190     MotionCommand * baseaddrs[MAX_ACCESS]; //!< for each accessor, the base address of the motion command
00191 #ifdef PLATFORM_APERIOS
00192     RCRegion * rcr[MAX_ACCESS];            //!< for each accessor the shared memory region that holds the motion command
00193 #endif
00194     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)
00195     MutexLock<MAX_ACCESS> lock;            //!< a lock to maintain mutual exclusion
00196     float priority;                        //!< MotionCommand's priority level
00197   private:
00198     CommandEntry(const CommandEntry&); //!< this shouldn't be called...
00199     CommandEntry& operator=(const CommandEntry&); //!< this shouldn't be called...
00200   };
00201   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!
00202   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
00203 
00204 
00205   inline MC_ID pop_free() { return cmdlist.new_front(); } //!<pulls an entry from cmdlist's free section and returns its index
00206   inline void push_free(MC_ID a) { cmdlist.erase(a); }    //!<puts an entry back into cmdlist's free section
00207 
00208   MutexLock<MAX_ACCESS> MMlock;          //!< The main lock for the class
00209 
00210   typedef ListMemBuf<OutputState,MAX_MOTIONS> cmdstatelist_t; //!< shorthand for a list of OutputState's
00211   cmdstatelist_t cmdstates[NumOutputs];  //!< requested positions by each of the MC's for each of the outputs
00212   float cmdSums[NumOutputs];             //!<Holds the final values for the outputs of the last frame generated
00213   OutputCmd cmds[NumOutputs];            //!<Holds the weighted values and total weight for the outputs of the last frame
00214 
00215 #ifdef PLATFORM_APERIOS
00216   accID_t numAcc;                        //!<The number of accessors who have registered with InitAccess()
00217   OSubject* subjs[MAX_ACCESS];           //!<The OSubject for each process (accessor) on which it should be broadcast when a command is added
00218 #endif
00219 
00220   static unsigned int _MMaccID;          //!<Stores the accessor for the current process
00221 
00222 private:
00223   MotionManager(const MotionManager&); //!< this shouldn't be called...
00224   MotionManager& operator=(const MotionManager&); //!< this shouldn't be called...
00225 };
00226 
00227 //!anyone who #includes MotionManager.h will be wanting to use the global motman... don't want multiple of these! created by MotoObj
00228 extern MotionManager * motman;
00229 
00230 /*! @file
00231  * @brief Describes MotionManager, simplifies sharing of MotionCommand's and provides mutual exclusion to their access
00232  * @author ejt (Creator)
00233  *
00234  * $Author: ejt $
00235  * $Name: tekkotsu-1_4_1 $
00236  * $Revision: 1.13 $
00237  * $State: Exp $
00238  * $Date: 2003/04/25 18:22:40 $
00239  */
00240 
00241 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:31 2003 by Doxygen 1.3.2