Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

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

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