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

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:38 2016 by Doxygen 1.6.3