Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ERS2xxInfo.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 // Mad props to Daishi MORI, the 220 master chief, for porting to the 220 ;)
00004 
00005 #ifndef INCLUDED_ERS2xxInfo_h
00006 #define INCLUDED_ERS2xxInfo_h
00007 
00008 #include "CommonERSInfo.h"
00009 #include "ERS210Info.h"
00010 #include "ERS220Info.h"
00011 #include <iostream>
00012 
00013 // see http://tekkotsu.org/porting.html#configuration for more information on TGT_HAS_* flags
00014 #if defined(TGT_ERS2xx)
00015 # define TGT_HAS_LEDS 22
00016 # define TGT_HAS_BUTTONS 11
00017 # define TGT_HAS_IR_DISTANCE 1
00018 #endif
00019 
00020 //! Contains information about the ERS-2xx series of robot, such as number of joints, PID defaults, timing information, etc.
00021 /*! This is a compatability mode, which allows dual-booting
00022  *  of the same memory stick on both models without needing
00023  *  to recompile, at the expense of (a little) runtime speed. */
00024 namespace ERS2xxInfo {
00025 
00026   // *******************************
00027   //       ROBOT CONFIGURATION
00028   // *******************************
00029 
00030   extern const char* const TargetName; //!< the name of the model, to be used for logging and remote GUIs
00031 
00032   const unsigned int FrameTime=8;        //!< time between frames in the motion system (milliseconds)
00033   const unsigned int NumFrames=4;        //!< the number of frames per buffer (don't forget also double buffered)
00034   const unsigned int SlowFrameTime=128;  //!< time between frames for the ears (which move slower for some reason, don't want to mix with other outputs) (milliseconds)
00035   const unsigned int NumSlowFrames=1;    //!< the number of frames per buffer being sent to ears (double buffered as well)
00036   const unsigned int SoundBufferTime=32; //!< the number of milliseconds per sound buffer... I'm not sure if this can be changed
00037   
00038   //!Corresponds to entries in ERS2xxInfo::PrimitiveName, defined at the end of this file, these are the primary grouping
00039   /*!Right now all binary joints are slow, but perhaps this won't always be the case... hence the IsFast/Slow bitmasks to select which type, in order to be more general */
00040   //!@name Output Types Information
00041   const unsigned NumWheels = 0; //!< no wheels, just legs
00042   
00043   const unsigned JointsPerArm   =  0; //!< no arms, just legs
00044   const unsigned NumArms        =  0; //!< no arms, just legs
00045   const unsigned NumArmJoints   =  JointsPerArm*NumArms;
00046   
00047   const unsigned JointsPerLeg   =  3; //!< The number of joints per leg
00048   const unsigned NumLegs        =  4; //!< The number of legs
00049   const unsigned NumLegJoints   =  JointsPerLeg*NumLegs; //!< the TOTAL number of joints on ALL legs
00050   const unsigned NumHeadJoints  =  3; //!< The number of joints in the neck
00051   const unsigned NumTailJoints  =  2; //!< The number of joints assigned to the tail
00052   const unsigned NumMouthJoints =  1; //!< the number of joints that control the mouth
00053   const unsigned NumEarJoints   =  2; //!< The number of joints which control the ears (NOT per ear, is total)
00054   const unsigned NumButtons     =  11; //!< the number of buttons that are available, see ERS2xxInfo::ButtonOffset_t
00055   const unsigned NumSensors     =  1+3+1+5;  //!< 1 dist, 3 accel, 1 thermo, 5 from power, see ERS2xxInfo::SensorOffset_t
00056   const unsigned NumLEDs        =  22; //!< The number of LEDs which can be controlled
00057   
00058   const unsigned NumPIDJoints   = NumLegJoints+NumHeadJoints+NumTailJoints+NumMouthJoints; //!< The number of joints which use PID motion - everything except ears
00059   const unsigned NumBinJoints   = NumEarJoints; //!< The number of binary joints - just the ears (Aperios only)
00060   const unsigned NumOutputs     = NumPIDJoints + NumBinJoints + NumLEDs; //!< the total number of outputs
00061   const unsigned NumReferenceFrames = NumOutputs + 1 + NumLegs + 1 + 1; //!< for the base, paw, camera, and IR sensor reference frames
00062 
00063   const float CameraHorizFOV=57.6/180*M_PI; //!< horizontal field of view (radians)
00064   const float CameraVertFOV=47.8/180*M_PI; //!< vertical field of view (radians)
00065   const float CameraFOV=CameraHorizFOV; //!< should be set to maximum of #CameraHorizFOV or #CameraVertFOV
00066   const unsigned int CameraResolutionX=176; //!< the number of pixels available in the 'full' layer
00067   const unsigned int CameraResolutionY=144; //!< the number of pixels available in the 'full' layer
00068   const float BallOfFootRadius=27.922/2; //!< radius of the ball of the foot
00069   const float CylinderOfFootRadius=24.606/2; //!< radius of the cylinder of the foot
00070 
00071   //! true for joints which can be updated every 32 ms (all but the ears on a 210)
00072   const bool IsFastOutput[NumOutputs] = {
00073     // for PID joints
00074     true, true, true, //legs
00075     true, true, true,
00076     true, true, true,
00077     true, true, true,
00078     true, true, true, //head
00079     true, true,       //tail
00080     true,             //mouth
00081     // for LEDs
00082     true, true, true,           // face left side LEDs x3
00083     true, true, true,           // face right side LEDs x3
00084     true,                       // head mode LED x1
00085     true, true, true,           // back left multi LEDs x3
00086     true, true, true,           // back right multi LEDs x3
00087     true, true, true,           // tail LEDs x3
00088     true, true, true,           // face front LEDs x3
00089     true,                       // retractable head light x1
00090     true, true,                 // tail red/blue from 210
00091     // for binary joints
00092     false, false
00093   };
00094   
00095   
00096   
00097   // *******************************
00098   //         OUTPUT OFFSETS
00099   // *******************************
00100 
00101 
00102   //!Corresponds to entries in ERS2xxInfo::PrimitiveName, defined at the end of this file
00103   //!@name Output Offsets
00104   const unsigned PIDJointOffset = 0; //!< The beginning of the PID Joints
00105   const unsigned LegOffset   = PIDJointOffset;           //!< the offset of the beginning of the leg joints
00106   const unsigned HeadOffset  = LegOffset+NumLegJoints;   //!< the offset of the beginning of the head joints
00107   const unsigned TailOffset  = HeadOffset+NumHeadJoints; //!< the offset of the beginning of the tail joints
00108   const unsigned MouthOffset = TailOffset+NumTailJoints; //!< the offset of the beginning of the mouth joint
00109 
00110   const unsigned LEDOffset   = PIDJointOffset + NumPIDJoints; //!< the offset of LEDs in WorldState::outputs and MotionCommand functions
00111 
00112   const unsigned BinJointOffset = LEDOffset+NumLEDs; //!< The beginning of the binary joints
00113   const unsigned EarOffset   = BinJointOffset;           //!< the offset of the beginning of the ear joints - note that ears aren't sensed.  They can be flicked by the environment and you won't know.  Nor will they be flicked back
00114 
00115   const unsigned BaseFrameOffset   = NumOutputs; //!< Use with kinematics to refer to base reference frame
00116   const unsigned PawFrameOffset    = BaseFrameOffset+1; //!< Use with kinematics to refer to paw reference frames (add appropriate LegOrder_t to specify which paw)
00117   const unsigned CameraFrameOffset = PawFrameOffset+NumLegs; //!< Use with kinematics to refer to camera reference frame
00118   const unsigned IRFrameOffset = CameraFrameOffset+1; //!< Use with kinematics to refer to infrared (distance) sensor reference frame
00119 
00120   //! the ordering of legs
00121   enum LegOrder_t {
00122     LFrLegOrder = 0, //!< left front leg
00123     RFrLegOrder,     //!< right front leg
00124     LBkLegOrder,     //!< left back leg
00125     RBkLegOrder      //!< right back leg
00126   };
00127   
00128   //! The offsets within appendages (the legs)  Note that the ordering matches the actual physical ordering of joints on the appendage (and not that of the head's TPROffset_t's)
00129   enum REKOffset_t {
00130     RotatorOffset=0, //!< moves leg forward or backward along body
00131     ElevatorOffset,  //!< moves leg toward or away from body
00132     KneeOffset       //!< moves knee
00133   };
00134   
00135   //! The offsets of appendages with tilt (elevation), pan (heading), and roll or nod joints (i.e. head)  Note that the ordering matches the actual physical ordering of joints on the appendage (and not that of the leg's REKOffset_t's)
00136   enum TPROffset_t {
00137     TiltOffset = 0, //!< tilt/elevation (vertical)
00138     PanOffset,      //!< pan/heading (horizontal)
00139     RollOffset,      //!< roll (rotational)
00140     NodOffset=RollOffset       //!< nod (second tilt)
00141   };
00142   
00143   //! The offsets of the individual legs
00144   enum LegOffset_t {
00145     LFrLegOffset = LegOffset+LFrLegOrder*JointsPerLeg, //!< beginning of left front leg
00146     RFrLegOffset = LegOffset+RFrLegOrder*JointsPerLeg, //!< beginning of right front leg
00147     LBkLegOffset = LegOffset+LBkLegOrder*JointsPerLeg, //!< beginning of left back leg
00148     RBkLegOffset = LegOffset+RBkLegOrder*JointsPerLeg  //!< beginning of right back leg
00149   };
00150   
00151   //@}
00152   
00153   //! The offsets of the individual LEDs on the head and tail.  Note that left/right are robot's point of view.  See also LEDBitMask_t
00154   enum LEDOffset_t {
00155     FaceFrontLeftLEDOffset = LEDOffset, //!< head face side light (front left - blue) (ERS-220)
00156     FaceFrontRightLEDOffset,    //!< head face side light (front right - blue) (ERS-220)
00157     FaceCenterLeftLEDOffset,    //!< head face side light (center left - blue) (ERS-220)
00158     FaceCenterRightLEDOffset,   //!< head face side light (center right - blue) (ERS-220)
00159     FaceBackLeftLEDOffset,      //!< head face side light (back left - red) (ERS-220)
00160     FaceBackRightLEDOffset,     //!< head face side light (back right - red) (ERS-220)
00161     ModeLEDOffset,              //!< mode indicator (back of the head - orange) (ERS-220)
00162     BackLeft1LEDOffset,         //!< back multi-indicator (left #1 - blue) (ERS-220)
00163     BackLeft2LEDOffset,         //!< back multi-indicator (left #2 - blue) (ERS-220)
00164     BackLeft3LEDOffset,         //!< back multi-indicator (left #3 - blue) (ERS-220)
00165     BackRight3LEDOffset,        //!< back multi-indicator (right #3 - blue) (ERS-220)
00166     BackRight2LEDOffset,        //!< back multi-indicator (right #2 - blue) (ERS-220)
00167     BackRight1LEDOffset,        //!< back multi-indicator (right #1 - blue) (ERS-220)
00168     TailLeftLEDOffset,          //!< tail light (left - blue) (ERS-220)
00169     TailCenterLEDOffset,        //!< tail light (center - red) (ERS-220)
00170     TailRightLEDOffset,         //!< tail light (right - blue) (ERS-220)
00171     FaceFrontBLEDOffset,        //!< face front light B (blue) (ERS-220)
00172     FaceFrontALEDOffset,        //!< face front light A (blue) (ERS-220)
00173     FaceFrontCLEDOffset,        //!< face front light C (red) (ERS-220)
00174     RetractableHeadLEDOffset,   //!< retractable head light (ERS-220)
00175  
00176     TlBluLEDOffset,             //!< blue tail light (ERS-210)
00177     TlRedLEDOffset,             //!< red tail light (ERS-210)
00178 
00179     // aliases for backward compatibility
00180     BotLLEDOffset = FaceFrontLeftLEDOffset,   //!< bottom left (red - sad) (ERS-210)
00181     BotRLEDOffset = FaceFrontRightLEDOffset,  //!< bottom right (red - sad) (ERS-210)
00182     MidLLEDOffset = FaceCenterLeftLEDOffset,  //!< middle left (green - happy) (ERS-210)
00183     MidRLEDOffset = FaceCenterRightLEDOffset, //!< middle right (green - happy) (ERS-210)
00184     TopLLEDOffset = FaceBackLeftLEDOffset,    //!< top left (red - angry) (ERS-210)
00185     TopRLEDOffset = FaceBackRightLEDOffset,   //!< top right (red - angry) (ERS-210)
00186     TopBrLEDOffset = ModeLEDOffset,           //!< top bar (green) (ERS-210)
00187   };
00188   
00189   //! Bitmasks for use when specifying combinations of LEDs (see LEDEngine ) Note that left/right are robot's point of view
00190   //!@name LED Bitmasks
00191   typedef unsigned int LEDBitMask_t; //!< So you can be clear when you're refering to a LED bitmask
00192   const LEDBitMask_t FaceFrontLeftLEDMask   = 1<<(FaceFrontLeftLEDOffset-LEDOffset);
00193   const LEDBitMask_t FaceFrontRightLEDMask  = 1<<(FaceFrontRightLEDOffset-LEDOffset);
00194   const LEDBitMask_t FaceCenterLeftLEDMask  = 1<<(FaceCenterLeftLEDOffset-LEDOffset);
00195   const LEDBitMask_t FaceCenterRightLEDMask = 1<<(FaceCenterRightLEDOffset-LEDOffset);
00196   const LEDBitMask_t FaceBackLeftLEDMask    = 1<<(FaceBackLeftLEDOffset-LEDOffset);
00197   const LEDBitMask_t FaceBackRightLEDMask   = 1<<(FaceBackRightLEDOffset-LEDOffset);
00198   const LEDBitMask_t ModeLEDMask            = 1<<(ModeLEDOffset-LEDOffset);
00199   const LEDBitMask_t BackLeft1LEDMask       = 1<<(BackLeft1LEDOffset-LEDOffset);
00200   const LEDBitMask_t BackLeft2LEDMask       = 1<<(BackLeft2LEDOffset-LEDOffset);
00201   const LEDBitMask_t BackLeft3LEDMask       = 1<<(BackLeft3LEDOffset-LEDOffset);
00202   const LEDBitMask_t BackRight3LEDMask      = 1<<(BackRight3LEDOffset-LEDOffset);
00203   const LEDBitMask_t BackRight2LEDMask      = 1<<(BackRight2LEDOffset-LEDOffset);
00204   const LEDBitMask_t BackRight1LEDMask      = 1<<(BackRight1LEDOffset-LEDOffset);
00205   const LEDBitMask_t TailLeftLEDMask        = 1<<(TailLeftLEDOffset-LEDOffset);
00206   const LEDBitMask_t TailCenterLEDMask      = 1<<(TailCenterLEDOffset-LEDOffset);
00207   const LEDBitMask_t TailRightLEDMask       = 1<<(TailRightLEDOffset-LEDOffset);
00208   const LEDBitMask_t FaceFrontBLEDMask      = 1<<(FaceFrontBLEDOffset-LEDOffset);
00209   const LEDBitMask_t FaceFrontALEDMask      = 1<<(FaceFrontALEDOffset-LEDOffset);
00210   const LEDBitMask_t FaceFrontCLEDMask      = 1<<(FaceFrontCLEDOffset-LEDOffset);
00211   const LEDBitMask_t RetractableHeadLEDMask = 1<<(RetractableHeadLEDOffset-LEDOffset);
00212   
00213   const LEDBitMask_t TlRedLEDMask= 1<<(TlRedLEDOffset-LEDOffset); //!< red tail light
00214   const LEDBitMask_t TlBluLEDMask= 1<<(TlBluLEDOffset-LEDOffset); //!< blue tail light
00215 
00216   // aliases for backward compatibility
00217   const LEDBitMask_t BotLLEDMask = 1<<(BotLLEDOffset-LEDOffset); //!< bottom left (red - sad)
00218   const LEDBitMask_t BotRLEDMask = 1<<(BotRLEDOffset-LEDOffset); //!< bottom right (red - sad)
00219   const LEDBitMask_t MidLLEDMask = 1<<(MidLLEDOffset-LEDOffset); //!< middle left (green - happy)
00220   const LEDBitMask_t MidRLEDMask = 1<<(MidRLEDOffset-LEDOffset); //!< middle right (green - happy)
00221   const LEDBitMask_t TopLLEDMask = 1<<(TopLLEDOffset-LEDOffset); //!< top left (red - angry)
00222   const LEDBitMask_t TopRLEDMask = 1<<(TopRLEDOffset-LEDOffset); //!< top right (red - angry)
00223   const LEDBitMask_t TopBrLEDMask= 1<<(TopBrLEDOffset-LEDOffset); //!< top bar (green)
00224 
00225   //! LEDs for face
00226   const LEDBitMask_t FaceLEDMask
00227   = FaceFrontLeftLEDMask
00228   | FaceFrontRightLEDMask
00229   | FaceCenterLeftLEDMask
00230   | FaceCenterRightLEDMask
00231   | FaceBackLeftLEDMask
00232   | FaceBackRightLEDMask
00233   | FaceFrontALEDMask
00234   | FaceFrontBLEDMask
00235   | FaceFrontCLEDMask
00236   | ModeLEDMask;
00237  
00238   //! LEDs on head (face plus retractable light)
00239   const LEDBitMask_t HeadLEDMask
00240   = FaceLEDMask | RetractableHeadLEDMask;
00241  
00242   //! LEDs on back
00243   const LEDBitMask_t BackLEDMask
00244   = BackLeft1LEDMask
00245   | BackLeft2LEDMask
00246   | BackLeft3LEDMask
00247   | BackRight1LEDMask
00248   | BackRight2LEDMask
00249   | BackRight3LEDMask;
00250  
00251   //! LEDs for tail
00252   const LEDBitMask_t TailLEDMask
00253   = TailLeftLEDMask
00254   | TailCenterLEDMask
00255   | TailRightLEDMask
00256   | TlRedLEDMask
00257   | TlBluLEDMask;
00258  
00259   //! selects all of the leds
00260   const LEDBitMask_t AllLEDMask  = (LEDBitMask_t)~0;
00261   //@}
00262   
00263   
00264   
00265   // *******************************
00266   //          INPUT OFFSETS
00267   // *******************************
00268 
00269   //! The order in which inputs should be stored
00270   //!@name Input Offsets
00271 
00272   //! holds offsets to different buttons in WorldState::buttons[]
00273   /*! Should be a straight mapping to the ButtonSourceIDs
00274    *
00275    *  Note that the chest (power) button is not a normal button.  It kills
00276    *  power to the motors at a hardware level, and isn't sensed in the
00277    *  normal way.  If you want to know when it is pressed (and you are
00278    *  about to shut down) see PowerSrcID::PauseSID.
00279    *
00280    *  @see WorldState::buttons @see ButtonSourceID_t */
00281   enum ButtonOffset_t {
00282     LFrPawOffset = LFrLegOrder,
00283     RFrPawOffset = RFrLegOrder,
00284     LBkPawOffset = LBkLegOrder,
00285     RBkPawOffset = RBkLegOrder,
00286     ChinButOffset= 4,
00287     BackButOffset,
00288     HeadFrButOffset, //!< see ERS210Info::HeadFrButOffset, ERS220Info::HeadFrButOffset
00289     HeadButOffset=HeadFrButOffset,  //!< for ERS-7 compatibility
00290     HeadBkButOffset, //!< see ERS210Info::HeadBkButOffset, ERS220Info::HeadBkButOffset
00291     TailLeftButOffset,
00292     TailCenterButOffset,
00293     TailRightButOffset,
00294   };
00295 
00296   //! Provides a string name for each button
00297   const char* const buttonNames[NumButtons] = {
00298     "LFrPaw","RFrPaw","LBkPaw","RBkPaw",
00299     "ChinBut","BackBut","HeadFrBut","HeadBkBut",
00300     "TailLeftBut","TailCenterBut","TailRightBut"
00301   };
00302 
00303   //! holds offset to different sensor values in WorldState::sensors[]
00304   /*! @see WorldState::sensors[] */
00305   enum SensorOffset_t {
00306     IRDistOffset = 0,  //!< in millimeters
00307     BAccelOffset, //!< backward acceleration, in @f$m/s^2@f$, negative if sitting on butt (positive for faceplant)
00308     LAccelOffset, //!< acceleration to the robot's left, in @f$m/s^2@f$, negative if lying on robot's left side
00309     DAccelOffset, //!< downward acceleration, in @f$m/s^2@f$, negative if standing up... be careful about the signs on all of these...
00310     ThermoOffset, //!< in degrees Celcius
00311     PowerRemainOffset, //!< percentage, 0-1
00312     PowerThermoOffset, //!<  degrees Celcius
00313     PowerCapacityOffset, //!< milli-amp hours
00314     PowerVoltageOffset, //!< volts
00315     PowerCurrentOffset //!< milli-amp negative values (maybe positive while charging?)
00316   };
00317 
00318   //! Provides a string name for each sensor
00319   const char* const sensorNames[NumSensors] = {
00320     "IRDist",
00321     "BAccel","LAccel","DAccel",
00322     "Thermo",
00323     "PowerRemain","PowerThermo","PowerCapacity","PowerVoltage","PowerCurrent"
00324   };
00325 
00326   //@}
00327 
00328 
00329   //! Names for each of the outputs
00330   const char* const outputNames[NumOutputs] = {
00331     "LFr:rotor",
00332     "LFr:elvtr",
00333     "LFr:knee",
00334     "RFr:rotor",
00335     "RFr:elvtr",
00336     "RFr:knee",
00337     "LBk:rotor",
00338     "LBk:elvtr",
00339     "LBk:knee",
00340     "RBk:rotor",
00341     "RBk:elvtr",
00342     "RBk:knee",
00343     
00344     "NECK:tilt",
00345     "NECK:pan",
00346     "NECK:roll",
00347     
00348     "TAIL:tilt",
00349     "TAIL:pan",
00350     
00351     "MOUTH",
00352     
00353     "LED:botL",
00354     "LED:botR",
00355     "LED:midL",
00356     "LED:midR",
00357     "LED:topL",
00358     "LED:topR",
00359     "LED:topBr",
00360     
00361     "LED:bkL1",                // "LED:tlBlu" of ERS-210
00362     "LED:bkL2",                // "LED:tlRed" of ERS-210
00363     "LED:bkL3",
00364     "LED:bkR3",
00365     "LED:bkR2",
00366     "LED:bkR1",
00367     "LED:tailL",
00368     "LED:tailC",
00369     "LED:tailR",
00370     "LED:faceB",
00371     "LED:faceA",
00372     "LED:faceC",
00373     "LED:light",                 // retractable head light
00374     
00375     "LED:tlRed",
00376     "LED:tlBlu",
00377     
00378     "EAR:left",
00379     "EAR:right"
00380   };
00381   
00382   
00383   //! provides polymorphic robot capability detection/mapping
00384   class ERS2xxCapabilities : public Capabilities {
00385   public:
00386     //! constructor
00387     ERS2xxCapabilities()
00388     : Capabilities(TargetName,NumOutputs,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs)
00389     {
00390       // ers-7 button alias
00391       buttonToIndex["HeadBut"]=HeadButOffset;
00392 #ifdef TGT_ERS2xx
00393       if(detectModel() == ERS210Info::TargetName) {
00394         fakeOutputs.insert(BackLeft1LEDOffset);
00395         fakeOutputs.insert(BackLeft2LEDOffset);
00396         fakeOutputs.insert(BackLeft3LEDOffset);
00397         fakeOutputs.insert(BackRight3LEDOffset);
00398         fakeOutputs.insert(BackRight2LEDOffset);
00399         fakeOutputs.insert(BackRight1LEDOffset);
00400         fakeOutputs.insert(TailLeftLEDOffset);
00401         fakeOutputs.insert(TailCenterLEDOffset);
00402         fakeOutputs.insert(TailRightLEDOffset);
00403         fakeOutputs.insert(FaceFrontBLEDOffset);
00404         fakeOutputs.insert(FaceFrontALEDOffset);
00405         fakeOutputs.insert(FaceFrontCLEDOffset);
00406         fakeOutputs.insert(RetractableHeadLEDOffset);
00407       } else if(detectModel() == ERS220Info::TargetName) {
00408         for(unsigned int i=TailOffset; i<TailOffset+NumTailJoints; ++i)
00409           fakeOutputs.insert(i);
00410         for(unsigned int i=MouthOffset; i<MouthOffset+NumMouthJoints; ++i)
00411           fakeOutputs.insert(i);
00412         fakeOutputs.insert(TlBluLEDOffset);
00413         fakeOutputs.insert(TlRedLEDOffset);
00414         for(unsigned int i=EarOffset; i<EarOffset+NumEarJoints; ++i)
00415           fakeOutputs.insert(i);
00416       } else {
00417         std::cerr << "ERS2xxCapabilities running on unknown model!" << std::endl;
00418       }
00419 #endif
00420     }
00421   };
00422   //! allocation declared in RobotInfo.cc
00423   extern ERS2xxCapabilities capabilities;
00424   
00425   
00426   //! the joint identifier strings used to refer to specific joints in OPEN-R (but not needed for others)
00427   /*!@showinitializer 
00428    * @warning IMPORTANT!!!!  DO NOT CHANGE THE ORDER OF ITEMS IN THIS TABLE!!!\n
00429    *
00430    * The offset consts defined in this file correspond to this table and will make life easier
00431    * if you feel the need to reorder things, but they aren't used perfect @e everywhere \n
00432    * In particular, assumptions are made that the pid joints will be in slots 0-numPIDJoints
00433    * and that the fast outputs (ie NOT ears) will be in slots 0-NumFastOutputs\n
00434    * There may be other assumptions not noted here!!!
00435    * @note These entries DON'T correspond to the CPC index numbers defined in WorldState (this only lists joints, and in a different order defined by OPEN-R, that one has sensors as well*/
00436   const char* const PrimitiveName [NumOutputs] = {
00437     "PRM:/r2/c1-Joint2:j1",       //!< the left front leg   the rotator
00438     "PRM:/r2/c1/c2-Joint2:j2",    //!< the left front leg   the elevator 
00439     "PRM:/r2/c1/c2/c3-Joint2:j3", //!< the left front leg   the knee 
00440     "PRM:/r4/c1-Joint2:j1",       //!< the right front leg   the rotator
00441     "PRM:/r4/c1/c2-Joint2:j2",    //!< the right front leg    the elevator 
00442     "PRM:/r4/c1/c2/c3-Joint2:j3", //!< the right front leg   the knee 
00443     
00444     "PRM:/r3/c1-Joint2:j1",       //!< the left hind leg   the rotator
00445     "PRM:/r3/c1/c2-Joint2:j2",    //!< the left hind leg   the elevator 
00446     "PRM:/r3/c1/c2/c3-Joint2:j3", //!< the left hind leg   the knee
00447     "PRM:/r5/c1-Joint2:j1",       //!< the right hind leg   the rotator
00448     "PRM:/r5/c1/c2-Joint2:j2",    //!< the right hind leg   the elevator 
00449     "PRM:/r5/c1/c2/c3-Joint2:j3", //!< the right hind leg   the knee 
00450 
00451     "PRM:/r1/c1-Joint2:j1",       //!< the neck  tilt (12)
00452     "PRM:/r1/c1/c2-Joint2:j2",    //!< the neck   pan 
00453     "PRM:/r1/c1/c2/c3-Joint2:j3", //!< the neck   roll 
00454         
00455     "PRM:/r6/c2-Joint2:j2",       //!< the tail tilt (15)       // *** NOTE ***
00456     "PRM:/r6/c1-Joint2:j1",       //!< the tail pan             // CHANGE_ET 12/16/01 matches neck order
00457     
00458     "PRM:/r1/c1/c2/c3/c4-Joint2:j4", //!< the mouth (17)
00459     
00460     "PRM:/r1/c1/c2/c3/l1-LED2:l1", //!< lower  left  LED (18)
00461     "PRM:/r1/c1/c2/c3/l4-LED2:l4", //!< lower  right LED
00462     "PRM:/r1/c1/c2/c3/l2-LED2:l2", //!< middle left  LED
00463     "PRM:/r1/c1/c2/c3/l5-LED2:l5", //!< middle right LED
00464     "PRM:/r1/c1/c2/c3/l3-LED2:l3", //!< upper  left  LED
00465     "PRM:/r1/c1/c2/c3/l6-LED2:l6", //!< upper  right LED
00466     "PRM:/r1/c1/c2/c3/l7-LED2:l7", //!< top          LED
00467     
00468     "PRM:/r6/l1-LED2:l1", //!< back 1st left LED (corresponds to tail blue LED of ERS-210) (25)
00469     "PRM:/r6/l2-LED2:l2", //!< back 2nd left LED (corresponds to tail red  LED of ERS-210)
00470     "PRM:/r6/l3-LED2:l3", //!< back 3rd left LED
00471     "PRM:/r6/l4-LED2:l4", //!< back 3rd right LED
00472     "PRM:/r6/l5-LED2:l5", //!< back 2nd right LED
00473     "PRM:/r6/l6-LED2:l6", //!< back 1st right LED
00474  
00475     "PRM:/r6/l9-LED2:l9", //!< tail left LED   (31)
00476     "PRM:/r6/l7-LED2:l7", //!< tail center LED
00477     "PRM:/r6/l8-LED2:l8", //!< tail right LED
00478  
00479     "PRM:/r1/c1/c2/c3/l8-LED2:l8", //!< face front LED B
00480     "PRM:/r1/c1/c2/c3/l9-LED2:l9", //!< face front LED A
00481     "PRM:/r1/c1/c2/c3/la-LED2:la", //!< face front LED C
00482     "PRM:/r1/c1/c2/c3/lb-LED2:lb", //!< retractable head light
00483 
00484     "PRM:/r6/l2-LED2:l2", //!< tail red LED (corresponds to back 2nd left LED of ERS-220)  (38)
00485     "PRM:/r6/l1-LED2:l1", //!< tail blue LED  (corresponds to back 1st left LED of ERS-220)
00486 
00487     "PRM:/r1/c1/c2/c3/e1-Joint3:j5", //!< left ear (40)
00488     "PRM:/r1/c1/c2/c3/e2-Joint3:j6" //!< right ear
00489   };
00490 
00491   //! use to open speaker connectio with the system
00492   const char* const SpeakerLocator="PRM:/r1/c1/c2/c3/s1-Speaker:S1";
00493 
00494   //! use to open camera connection with the system
00495   const char* const CameraLocator="PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1";
00496 
00497   //Old PID table:
00498   /*const word Pid[NumPIDJoints][6] = {
00499     { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
00500     { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
00501     { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
00502     { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
00503     { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
00504     { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
00505     { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
00506     { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
00507     { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
00508     { 0x16, 0x04, 0x08, 0x0E, 0x02, 0x0F },
00509     { 0x14, 0x04, 0x06, 0x0E, 0x02, 0x0F },
00510     { 0x23, 0x04, 0x05, 0x0E, 0x02, 0x0F },
00511     
00512     { 0x0A, 0x08, 0x0C, 0x0E, 0x02, 0x0F },
00513     { 0x0D, 0x08, 0x0B, 0x0E, 0x02, 0x0F },
00514     { 0x10, 0x08, 0x0C, 0x0E, 0x02, 0x0F }, // P was 0x0C, updated as seen on https://www.openr.org/page1_2001/gain.html 8/13/2002
00515     
00516     { 0x0A, 0x00, 0x18, 0x0E, 0x02, 0x0F },
00517     { 0x07, 0x00, 0x11, 0x0E, 0x02, 0x0F },
00518     
00519     { 0x0E, 0x08, 0x10, 0x0E, 0x02, 0x0F }, //  { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },{ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 },
00520   };*/
00521       
00522   //! This table holds the default PID values for each joint.  see PIDMC
00523   const float DefaultPIDs[NumPIDJoints][3] =
00524     {
00525       { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00526       { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00527       { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00528       { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00529       { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00530       { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00531       { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00532       { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00533       { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00534       { 0x16/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x08/(double)(1<<(16-0xF)) },
00535       { 0x14/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x06/(double)(1<<(16-0xF)) },
00536       { 0x23/(double)(1<<(16-0xE)), 0x04/(double)(1<<(16-0x2)), 0x05/(double)(1<<(16-0xF)) },
00537 
00538       { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00539       { 0x0D/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0B/(double)(1<<(16-0xF)) },
00540       { 0x0A/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x0C/(double)(1<<(16-0xF)) },
00541 
00542       { 0x0A/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x18/(double)(1<<(16-0xF)) },
00543       { 0x07/(double)(1<<(16-0xE)), 0x00/(double)(1<<(16-0x2)), 0x11/(double)(1<<(16-0xF)) },
00544 
00545       { 0x0E/(double)(1<<(16-0xE)), 0x08/(double)(1<<(16-0x2)), 0x10/(double)(1<<(16-0xF)) }
00546     };
00547   
00548   //! These will control the shift values given to the system.  see PIDMC
00549   const unsigned char DefaultPIDShifts[3] = {0x0E, 0x02, 0x0F};
00550     
00551   //!These values are Sony's recommended maximum joint velocities, in rad/ms
00552   /*! a value <= 0 means infinite speed (e.g. LEDs)
00553    *  
00554    *  These limits are <b>not</b> enforced by the framework.  They are simply available for you to use as you see fit.
00555    *  HeadPointerMC is (as of v1.6) the only included MotionCommand to actually use these values. */
00556   const float MaxOutputSpeed[NumOutputs] = {
00557     2.8143434e-03,     //Legs LR,FB,REK
00558     2.4980025e-03,
00559     2.8361600e-03,
00560     2.8143434e-03,
00561     2.4980025e-03,
00562     2.8361600e-03,
00563     2.8143434e-03,
00564     2.4980025e-03,
00565     2.8361600e-03,
00566     2.8143434e-03,
00567     2.4980025e-03,
00568     2.8361600e-03,
00569   
00570     2.1053034e-03,     //Head TPR
00571     3.0106930e-03,
00572     3.0106930e-03,
00573   
00574     4.4724062e-03,     //Tail
00575     4.4724062e-03,
00576   
00577     4.3742314e-03,     //Mouth
00578     
00579     0,0,0,//LEDs
00580     0,0,0,
00581     0,
00582     0,0,0, 
00583     0,0,0, 
00584     0,0,0, 
00585     0,0,0, 
00586     0,
00587     0,0,
00588 
00589     0,0                //Ears
00590   };
00591 
00592 #ifndef RAD
00593   //!Just a little macro for converting degrees to radians
00594 #define RAD(deg) (((deg) * M_PI ) / 180.0)
00595   //!a flag so we undef these after we're done - do you have a cleaner solution?
00596 #define __RI_RAD_FLAG
00597 #endif
00598 
00599   //! This table holds the software limits of each of the outputs
00600   const double outputRanges[NumOutputs][2] =
00601     {
00602       { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) }, //left front REK
00603       { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) }, //right front REK
00604       { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) }, //left back REK
00605       { RAD(-117),RAD(117) },{ RAD(-11),RAD(89) },{ RAD(-27),RAD(147) }, //right back REK
00606 
00607       { RAD(-82),RAD(43) },{ RAD(-89.6),RAD(89.6) },{ RAD(-29),RAD(29) }, //neck TPR
00608         
00609       { RAD(-22),RAD(22) },{ RAD(-22),RAD(22) }, // tail tp
00610 
00611       { RAD(-47),RAD(-3) }, //mouth
00612 
00613       {0,1},{0,1},{0,1},        // face left side LEDs x3
00614       {0,1},{0,1},{0,1},        // face right side LEDs x3
00615       {0,1},                    // head mode LED x1
00616       {0,1},{0,1},{0,1},        // back left multi LEDs x3
00617       {0,1},{0,1},{0,1},        // back right multi LEDs x3
00618       {0,1},{0,1},{0,1},        // tail LEDs x3
00619       {0,1},{0,1},{0,1},        // face front LEDs x3
00620       {0,1},                    // retractable head light x1
00621       {0,1},{0,1},              // 210 red/blue tail light
00622 
00623       {0,1},{0,1} //ears
00624     };
00625 
00626   //! This table holds the mechanical limits of each of the outputs
00627   const double mechanicalLimits[NumOutputs][2] =
00628     {
00629       { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) }, //left front jsk
00630       { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) }, //right front jsk
00631       { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) }, //left back jsk
00632       { RAD(-120),RAD(120) },{ RAD(-14),RAD(92) },{ RAD(-30),RAD(150) }, //right back jsk
00633 
00634       { RAD(-85),RAD(46) },{ RAD(-92.6),RAD(92.6) },{ RAD(-32),RAD(32) }, //neck tpr
00635         
00636       { RAD(-25),RAD(25) },{ RAD(-25),RAD(25) }, // tail tp
00637 
00638       { RAD(-50),RAD(0) }, //mouth
00639         
00640       {0,1},{0,1},{0,1},        // face left side LEDs x3
00641       {0,1},{0,1},{0,1},        // face right side LEDs x3
00642       {0,1},                    // head mode LED x1
00643       {0,1},{0,1},{0,1},        // back left multi LEDs x3
00644       {0,1},{0,1},{0,1},        // back right multi LEDs x3
00645       {0,1},{0,1},{0,1},        // tail LEDs x3
00646       {0,1},{0,1},{0,1},        // face front LEDs x3
00647       {0,1},                     // retractable head light x1
00648       {0,1},{0,1},              // 210 red/blue tail light
00649 
00650       {0,1},{0,1} //ears
00651     };
00652 
00653 #ifdef __RI_RAD_FLAG
00654 #undef RAD
00655 #undef __RI_RAD_FLAG
00656 #endif
00657 
00658 }
00659 
00660 /*! @file
00661  * @brief Defines RobotInfo namespace for ERS-2xx series of robots, such as joint counts, offsets, names and PID values
00662  * @author Daishi MORI (Creator)
00663  *
00664  * $Author: ejt $
00665  * $Name: tekkotsu-4_0 $
00666  * $Revision: 1.32 $
00667  * $State: Exp $
00668  * $Date: 2007/11/09 19:01:14 $
00669  */
00670 
00671 #endif

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