Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ERS7Info.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_ERS7Info_h
00003 #define INCLUDED_ERS7Info_h
00004 
00005 #include "Shared/CommonERSInfo.h"
00006 
00007 // see http://tekkotsu.org/porting.html#configuration for more information on TGT_HAS_* flags
00008 #if defined(TGT_ERS7)
00009 # define TGT_HAS_LEDS 27
00010 # define TGT_HAS_BUTTONS 10
00011 # define TGT_HAS_IR_DISTANCE 3
00012 #endif
00013 
00014 //! Contains information about the ERS-7 Robot, such as number of joints, PID defaults, timing information, etc.
00015 /*! 
00016 You may be particularly interested in the "Output Offsets" section, which, along with the offsets of the common RobotInfo namespace,
00017 allows you to reference any specific joint or LED on the robot.
00018 
00019 The "Input Offsets" section gives the index order of the buttons (#ButtonOffset_t) and sensors (#SensorOffset_t), as well as
00020 string names for each for easier debugging (#buttonNames, #sensorNames)
00021 
00022 "Output Types" section provides "meta-information" regarding the capabilities of the robot, such as the number of head joints, or the number of LEDs, etc.
00023 
00024 For more information on your robot's specifications, see also #DefaultPIDs, #MaxOutputSpeed, #outputRanges, and #mechanicalLimits.
00025 
00026 "Outputs" (i.e. Joints, LEDs) are often refered to by index ("offset") value within an array.
00027 These values are formed by specifying a @e section offset, plus a @e specific offset.  Sections are typically general across robot models, whereas the specifics are model-dependent (but can be aliased to provide compatability).
00028 
00029 For most joints, the positive direction is "up", and the 0 position yields a forward looking, fully extended standing posture.
00030 
00031 - {L,R}{FrBk}LegOffset - #NumLegs combinations, each with #JointsPerLeg items, add REKOffset_t; see also LegOffset_t
00032   - + #RotatorOffset: positive moves "out", away from body
00033   - + #ElevatorOffset: positive moves up, away from body
00034   - + #KneeOffset: positive bends knee (slight negative possible)
00035 - #HeadOffset - #NumHeadJoints items, add #TPROffset_t:
00036   - + #TiltOffset: positive looks up
00037   - + #PanOffset: positive looks left
00038   - + #NodOffset: positive looks up
00039 - #TailOffset - #NumTailJoints items, add #TPROffset_t:
00040   - + #TiltOffset: positive raises the joint (lowers the tail itself)
00041   - + #PanOffset: positive points the tail to the Aibo's right
00042 - MouthOffset - #NumMouthJoints items (no specific, only 1 joint)
00043 - LEDs: #NumLEDs items, see #LEDOffset_t; these are all direct offsets, and do not need to be added to anything else
00044 - #EarOffset - #NumEarJoints items (no specifics, first is left ear, second is right ear)
00045 
00046 It happens that these joints can also be grouped by the @e type of joint, so there are additionally a few other offsets that can be used in order to loop across a group of joints:
00047 - PIDJointOffset - #NumPIDJoints items, servos using PID control
00048 - LegOffset - #NumLegJoints items, a subset of PID servos corresponding to the leg joints
00049 - LEDOffset - #NumLEDs items
00050 - BinJointOffset - #NumBinJoints items, solenoids, such as the ears (if any) which flip between two positions
00051 - #NumOutputs - total number of outputs available
00052 
00053 LEDs are often handled in groups to display patterns.  Some functions take an LEDBitMask_t
00054 parameter, which allows you to specify a set of several LEDs in a single parameter.  
00055 For any given LED offset @c fooLEDOffset, the corresponding bitmask constant is @c fooLEDMask.
00056 Alternatively, you could calculate the bitmask of @c fooLEDOffset by <code>1&lt;&lt;(fooLEDOffset-LEDOffset)</code>.
00057 
00058 @see <a href="../media/TekkotsuQuickReference_ERS7.pdf">ERS-7 Quick Reference Sheet</a>
00059 */
00060 namespace ERS7Info {
00061 
00062   // *******************************
00063   //       ROBOT CONFIGURATION
00064   // *******************************
00065 
00066   extern const char* const TargetName; //!< the name of the model, to be used for logging and remote GUIs
00067 
00068   const unsigned int FrameTime=8;        //!< time between frames in the motion system (milliseconds)
00069   const unsigned int NumFrames=4;        //!< the number of frames per buffer (don't forget also double buffered)
00070   const unsigned int SlowFrameTime=8;  //!< time between frames for the ears (ERS-7 doesn't seem to have any "slow" joints; this only applied for the ears on the ERS-210)
00071   const unsigned int NumSlowFrames=4;    //!< the number of frames per buffer being sent to ears (double buffered as well)
00072   const unsigned int SoundBufferTime=32; //!< the number of milliseconds per sound buffer... I'm not sure if this can be changed
00073   
00074   //!Corresponds to entries in ERS7Info::PrimitiveName, defined at the end of this file, these are the primary grouping
00075   /*!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 */
00076   //!@name Output Types Information
00077   const unsigned NumWheels = 0; //!< no wheels, just legs
00078   
00079   const unsigned JointsPerArm   =  0; //!< no arms, just legs
00080   const unsigned NumArms        =  0; //!< no arms, just legs
00081   const unsigned NumArmJoints   =  JointsPerArm*NumArms;
00082   
00083   const unsigned JointsPerLeg   =  3; //!< The number of joints per leg
00084   const unsigned NumLegs        =  4; //!< The number of legs
00085   const unsigned NumLegJoints   =  JointsPerLeg*NumLegs; //!< the TOTAL number of joints on ALL legs
00086   const unsigned NumHeadJoints  =  3; //!< The number of joints in the neck
00087   const unsigned NumTailJoints  =  2; //!< The number of joints assigned to the tail
00088   const unsigned NumMouthJoints =  1; //!< the number of joints that control the mouth
00089   const unsigned NumEarJoints   =  2; //!< The number of joints which control the ears (NOT per ear, is total)
00090   const unsigned NumButtons     =  2+4+3+1; //!< the number of buttons that are available, 2 head, 4 paws, 3 back, 1 underbelly see ERS7Info::ButtonOffset_t
00091   const unsigned NumSensors     =  3+3+5;  //!< 3 IR (distance), 3 accel (force), 5 from power, see ERS7Info::SensorOffset_t
00092   const unsigned NumLEDs        =  27; //!< The number of LEDs which can be controlled
00093   const unsigned NumFacePanelLEDs = 14; //!< The number of face panel LEDs
00094   
00095   const unsigned NumPIDJoints   = NumLegJoints+NumHeadJoints+NumTailJoints+NumMouthJoints; //!< The number of joints which use PID motion - everything except ears
00096   const unsigned NumBinJoints   = NumEarJoints; //!< The number of binary joints - just the ears
00097   const unsigned NumOutputs     = NumPIDJoints + NumBinJoints + NumLEDs; //!< the total number of outputs
00098   const unsigned NumReferenceFrames = NumOutputs + 1 + NumLegs + 1 + 3; //!< for the base, paws (NumLegs), camera, and IR sensors (3) reference frames
00099 
00100   const float CameraHorizFOV=56.9f/180*static_cast<float>(M_PI); //!< horizontal field of view (radians)
00101   const float CameraVertFOV=45.2f/180*static_cast<float>(M_PI); //!< vertical field of view (radians)
00102   const float CameraFOV=CameraHorizFOV; //!< should be set to maximum of #CameraHorizFOV or #CameraVertFOV
00103   const unsigned int CameraResolutionX=208; //!< the number of pixels available in the 'full' layer
00104   const unsigned int CameraResolutionY=160; //!< the number of pixels available in the 'full' layer
00105   extern const char CameraModelName[]; //!< specifies a name of the camera to load calibration parameters into RobotInfo::CameraHomography
00106   
00107   const float BallOfFootRadius=23.433f/2; //!< radius of the ball of the foot
00108 
00109   //! true for joints which can be updated every 32 ms (all joints on ERS-7)
00110   /*! @hideinitializer */
00111   const bool IsFastOutput[NumOutputs] = {
00112     true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true, //PID joints
00113     true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true, //leds
00114     true,true //ears
00115   };
00116   //@}
00117   
00118   
00119   
00120   // *******************************
00121   //         OUTPUT OFFSETS
00122   // *******************************
00123 
00124 
00125   //!Corresponds to entries in ERS7Info::PrimitiveName, defined at the end of this file
00126   //!@name Output Offsets
00127 
00128   const unsigned PIDJointOffset = 0; //!< The beginning of the PID Joints
00129   const unsigned LegOffset   = PIDJointOffset;           //!< the offset of the beginning of the leg joints, #NumLegs of #JointsPerLeg each, in #LegOrder_t order; see #LegOffset_t
00130   const unsigned HeadOffset  = LegOffset+NumLegJoints;   //!< the offset of the beginning of the head joints, add #TPROffset_t to get specific joint
00131   const unsigned TailOffset  = HeadOffset+NumHeadJoints; //!< the offset of the beginning of the tail joints, add #TPROffset_t to get specific joint (except RollOffset not available)
00132   const unsigned MouthOffset = TailOffset+NumTailJoints; //!< the offset of the beginning of the mouth joint, is specific joint
00133 
00134   const unsigned LEDOffset   = PIDJointOffset + NumPIDJoints; //!< the offset of LEDs in WorldState::outputs and MotionCommand functions, see LedOffset_t for specific offsets
00135 
00136   const unsigned BinJointOffset = LEDOffset + NumLEDs;   //!< The beginning of the binary joints
00137   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.
00138 
00139   const unsigned BaseFrameOffset   = NumOutputs; //!< Use with kinematics to refer to base reference frame
00140   const unsigned FootFrameOffset    = BaseFrameOffset+1; //!< Use with kinematics to refer to paw reference frames (add appropriate LegOrder_t to specify which paw)
00141   const unsigned PawFrameOffset    = FootFrameOffset; //!< Aibo-era alias for FootFrameOffset
00142   const unsigned CameraFrameOffset = FootFrameOffset+NumLegs; //!< Use with kinematics to refer to camera reference frame
00143   const unsigned NearIRFrameOffset = CameraFrameOffset+1; //!< Use with kinematics to refer to short-range infrared (distance) sensor reference frame
00144   const unsigned IRFrameOffset = NearIRFrameOffset; //!< alias for the near IR sensor
00145   const unsigned FarIRFrameOffset = NearIRFrameOffset+1; //!< Use with kinematics to refer to long-range infrared (distance) sensor reference frame
00146   const unsigned ChestIRFrameOffset = FarIRFrameOffset+1; //!< Use with kinematics to refer to chest-mounted infrared (distance) sensor reference frame
00147 
00148   //! the ordering of legs
00149   enum LegOrder_t {
00150     LFrLegOrder = 0, //!< left front leg
00151     RFrLegOrder,     //!< right front leg
00152     LBkLegOrder,     //!< left back leg
00153     RBkLegOrder      //!< right back leg
00154   };
00155   
00156   //! 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)
00157   enum REKOffset_t {
00158     RotatorOffset=0, //!< moves leg forward or backward along body
00159     ElevatorOffset,  //!< moves leg toward or away from body
00160     KneeOffset       //!< moves knee
00161   };
00162   
00163   //! 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)
00164   enum TPROffset_t {
00165     TiltOffset = 0, //!< tilt/elevation (vertical)
00166     PanOffset,      //!< pan/heading (horizontal)
00167     RollOffset,      //!< roll (rotational)
00168     NodOffset=RollOffset       //!< nod (second tilt)
00169   };
00170   
00171   //! The offsets of the individual legs, add #REKOffset_t value to access specific joint
00172   /*! @hideinitializer */
00173   enum LegOffset_t {
00174     LFrLegOffset = LegOffset+LFrLegOrder*JointsPerLeg, //!< beginning of left front leg's joints
00175     RFrLegOffset = LegOffset+RFrLegOrder*JointsPerLeg, //!< beginning of right front leg's joints
00176     LBkLegOffset = LegOffset+LBkLegOrder*JointsPerLeg, //!< beginning of left back leg's joints
00177     RBkLegOffset = LegOffset+RBkLegOrder*JointsPerLeg  //!< beginning of right back leg's joints
00178   };
00179   
00180   //! 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
00181   /*! @hideinitializer */
00182   enum LEDOffset_t {
00183     HeadColorLEDOffset = LEDOffset, //!< the orange LED immediately above the head button (copositioned with #HeadWhiteLEDOffset)
00184     HeadWhiteLEDOffset, //!< the LED immediately above the head button (copositioned with #HeadColorLEDOffset)
00185     ModeRedLEDOffset, //!< controls both of the red LEDs above the ears (copositioned with #ModeGreenLEDOffset and #ModeBlueLEDOffset)
00186     ModeGreenLEDOffset, //!< controls both of the green LEDs above the ears (copositioned with #ModeRedLEDOffset and #ModeBlueLEDOffset)
00187     ModeBlueLEDOffset, //!< controls both of the blue LEDs above the ears (copositioned with #ModeRedLEDOffset and #ModeGreenLEDOffset)
00188     WirelessLEDOffset, //!< controls the small rectangular blue LED on the back of the head
00189     FaceLEDPanelOffset, //!< the first LED in the panel - add 0 up to (not including) #NumFacePanelLEDs to this to access specific face panel LEDs, see LedEngine for diagram of placement
00190     FrBackColorLEDOffset = FaceLEDPanelOffset+NumFacePanelLEDs, //!< @b blue/purple LED on back, closest to head (copositioned with #FrBackWhiteLEDOffset)
00191     FrBackWhiteLEDOffset,  //!< white LED on back, closest to head (copositioned with #FrBackColorLEDOffset)
00192     MdBackColorLEDOffset, //!< @b orange LED on back, in between FrBackColorLEDOffset and RrBackColorLEDOffset (copositioned with #MdBackWhiteLEDOffset)
00193     MdBackWhiteLEDOffset, //!< white LED on back, in between FrBackWhiteLEDOffset and RrBackWhiteLEDOffset (copositioned with #MdBackColorLEDOffset)
00194     RrBackColorLEDOffset, //!< @b red LED on back, farthest from head (copositioned with #RrBackWhiteLEDOffset)
00195     RrBackWhiteLEDOffset, //!< white LED on back, farthest from head (copositioned with #RrBackColorLEDOffset)
00196     LEDABModeOffset, // allows you to control A/B mode setting (this is a "virtual" LED)
00197 
00198     // aliases for 2xx cross-compatibility
00199     BotLLEDOffset = FaceLEDPanelOffset+1,//!< aliases for backward compatability with ERS-210 (use mode A); bottom left of face panel
00200     BotRLEDOffset = FaceLEDPanelOffset+0, //!< aliases for backward compatability with ERS-210 (use mode A); bottom right of face panel
00201     MidLLEDOffset = FaceLEDPanelOffset+3, //!< aliases for backward compatability with ERS-210 (use mode A); middle left of face panel
00202     MidRLEDOffset = FaceLEDPanelOffset+2, //!< aliases for backward compatability with ERS-210 (use mode A); middle right of face panel
00203     TopLLEDOffset = FaceLEDPanelOffset+7, //!< aliases for backward compatability with ERS-210 (use mode A); top left of face panel
00204     TopRLEDOffset = FaceLEDPanelOffset+6, //!< aliases for backward compatability  with ERS-210(use mode A); top right of face panel
00205     TopBrLEDOffset= HeadColorLEDOffset,//!< aliases for backward compatability with ERS-210 (use mode A); top bar (#HeadColorLEDOffset)
00206     TlRedLEDOffset= RrBackColorLEDOffset,//!< aliases for backward compatability with ERS-210; red tail light (#RrBackColorLEDOffset)
00207     TlBluLEDOffset= FrBackColorLEDOffset //!< aliases for backward compatability with ERS-210; blue tail light (#FrBackColorLEDOffset)
00208   };
00209   //@}
00210     
00211   //! Bitmasks for use when specifying combinations of LEDs (see LedEngine ) Note that left/right are robot's point of view
00212   //!@name LED Bitmasks
00213   typedef unsigned int LEDBitMask_t; //!< So you can be clear when you're refering to a LED bitmask
00214   
00215   const LEDBitMask_t HeadColorLEDMask = 1<<(HeadColorLEDOffset-LEDOffset); //!< mask corresponding to HeadColorLEDOffset
00216   const LEDBitMask_t HeadWhiteLEDMask = 1<<(HeadWhiteLEDOffset-LEDOffset); //!< mask corresponding to HeadWhiteLEDOffset
00217   const LEDBitMask_t ModeRedLEDMask = 1<<(ModeRedLEDOffset-LEDOffset); //!< mask corresponding to ModeRedLEDOffset
00218   const LEDBitMask_t ModeGreenLEDMask = 1<<(ModeGreenLEDOffset-LEDOffset); //!< mask corresponding to ModeGreenLEDOffset
00219   const LEDBitMask_t ModeBlueLEDMask = 1<<(ModeBlueLEDOffset-LEDOffset); //!< mask corresponding to ModeBlueLEDOffset
00220   const LEDBitMask_t WirelessLEDMask = 1<<(WirelessLEDOffset-LEDOffset); //!< mask corresponding to WirelessLEDOffset
00221   const LEDBitMask_t FaceLEDPanelMask = 1<<(FaceLEDPanelOffset-LEDOffset); //!< mask corresponding to FaceLEDPanelOffset, selects only the first of the panel - shift this to get the others
00222   const LEDBitMask_t FrBackColorLEDMask = 1<<(FrBackColorLEDOffset-LEDOffset); //!< mask corresponding to FrBackColorLEDOffset
00223   const LEDBitMask_t FrBackWhiteLEDMask = 1<<(FrBackWhiteLEDOffset-LEDOffset); //!< mask corresponding to FrBackWhiteLEDOffset
00224   const LEDBitMask_t MdBackColorLEDMask = 1<<(MdBackColorLEDOffset-LEDOffset); //!< mask corresponding to MdBackColorLEDOffset
00225   const LEDBitMask_t MdBackWhiteLEDMask = 1<<(MdBackWhiteLEDOffset-LEDOffset); //!< mask corresponding to MdBackWhiteLEDOffset
00226   const LEDBitMask_t RrBackColorLEDMask = 1<<(RrBackColorLEDOffset-LEDOffset); //!< mask corresponding to RrBackColorLEDOffset
00227   const LEDBitMask_t RrBackWhiteLEDMask = 1<<(RrBackWhiteLEDOffset-LEDOffset); //!< mask corresponding to RrBackWhiteLEDOffset
00228   const LEDBitMask_t LEDABModeMask = 1<<(LEDABModeOffset-LEDOffset); //!< mask corresponding to LEDABModeOffset
00229 
00230   const LEDBitMask_t BotLLEDMask = 1<<(BotLLEDOffset-LEDOffset); //!< bottom left
00231   const LEDBitMask_t BotRLEDMask = 1<<(BotRLEDOffset-LEDOffset); //!< bottom right
00232   const LEDBitMask_t MidLLEDMask = 1<<(MidLLEDOffset-LEDOffset); //!< middle left
00233   const LEDBitMask_t MidRLEDMask = 1<<(MidRLEDOffset-LEDOffset); //!< middle right
00234   const LEDBitMask_t TopLLEDMask = 1<<(TopLLEDOffset-LEDOffset); //!< top left
00235   const LEDBitMask_t TopRLEDMask = 1<<(TopRLEDOffset-LEDOffset); //!< top right
00236   const LEDBitMask_t TopBrLEDMask= 1<<(TopBrLEDOffset-LEDOffset); //!< top bar
00237   const LEDBitMask_t TlRedLEDMask= 1<<(TlRedLEDOffset-LEDOffset); //!< red tail light
00238   const LEDBitMask_t TlBluLEDMask= 1<<(TlBluLEDOffset-LEDOffset); //!< blue tail light
00239 
00240   //! LEDs for the face panel (all FaceLEDPanelMask<<(0:NumFacePanelLEDs-1) entries)
00241   /*! @hideinitializer */
00242   const LEDBitMask_t FaceLEDMask = (FaceLEDPanelMask<<0) | (FaceLEDPanelMask<<1) | (FaceLEDPanelMask<<2) | (FaceLEDPanelMask<<3) | (FaceLEDPanelMask<<4) | (FaceLEDPanelMask<<5) | (FaceLEDPanelMask<<6) | (FaceLEDPanelMask<<7) | (FaceLEDPanelMask<<8) | (FaceLEDPanelMask<<9) | (FaceLEDPanelMask<<10) | (FaceLEDPanelMask<<11) | (FaceLEDPanelMask<<12) | (FaceLEDPanelMask<<13);
00243 
00244   //! LEDs for face (all but back lights)
00245   const LEDBitMask_t HeadLEDMask
00246   = FaceLEDMask | HeadColorLEDMask | HeadWhiteLEDMask
00247   | ModeRedLEDMask | ModeGreenLEDMask | ModeBlueLEDMask
00248   | WirelessLEDMask;
00249 
00250   //! LEDS on the back
00251   const LEDBitMask_t BackLEDMask
00252   = FrBackColorLEDMask | FrBackWhiteLEDMask
00253   | MdBackColorLEDMask | MdBackWhiteLEDMask
00254   | RrBackColorLEDMask | RrBackWhiteLEDMask;
00255 
00256   //! LEDs on tail (ERS-7 has none)
00257   const LEDBitMask_t TailLEDMask = 0;
00258 
00259   //! selects all of the leds
00260   const LEDBitMask_t AllLEDMask  = (LEDBitMask_t)~0;
00261   //@}
00262   
00263   
00264   //! Offset needed so that the centroid of the robot is correct relative to the bounding box
00265   const fmat::Column<3> AgentBoundingBoxBaseFrameOffset = fmat::pack(0,0,0); 
00266 
00267   //! Half of the length, width, and height of the robot
00268   const fmat::Column<3> AgentBoundingBoxHalfDims = fmat::pack(304.8/2, 304.8/2, 0);
00269   
00270   // *******************************
00271   //          INPUT OFFSETS
00272   // *******************************
00273 
00274 
00275   //! The order in which inputs should be stored
00276   //!@name Input Offsets
00277 
00278   //! holds offsets to different buttons in WorldState::buttons[]
00279   /*! Should be a straight mapping to the ButtonSourceIDs
00280    *
00281    *  Note that the chest (power) button is not a normal button.  It kills
00282    *  power to the motors at a hardware level, and isn't sensed in the
00283    *  normal way.  If you want to know when it is pressed (and you are
00284    *  about to shut down) see PowerSrcID::PauseSID.
00285    *
00286    *  @see WorldState::buttons @see ButtonSourceID_t
00287    * @hideinitializer */
00288   enum ButtonOffset_t {
00289     LFrPawOffset = LFrLegOrder,
00290     RFrPawOffset = RFrLegOrder,
00291     LBkPawOffset = LBkLegOrder,
00292     RBkPawOffset = RBkLegOrder,
00293     ChinButOffset= 4,
00294     HeadButOffset,
00295     HeadFrButOffset=HeadButOffset,
00296     FrontBackButOffset,
00297     MiddleBackButOffset,
00298     BackButOffset = MiddleBackButOffset,
00299     RearBackButOffset,
00300     WirelessSwOffset
00301   };
00302 
00303   //! Provides a string name for each button
00304   const char* const buttonNames[NumButtons] = {
00305     "LFrPaw","RFrPaw","LBkPaw","RBkPaw",
00306     "ChinBut","HeadBut",
00307     "FrontBackBut","MiddleBackBut","RearBackBut",
00308     "WirelessSw"
00309   };
00310 
00311   //! holds offset to different sensor values in WorldState::sensors[]
00312   /*! @see WorldState::sensors[] */
00313   enum SensorOffset_t {
00314     NearIRDistOffset = 0,  //!< in millimeters, ranges from 50 to 500
00315     IRDistOffset=NearIRDistOffset, //!< alias for ERS-2xx's solitary range finder
00316     FarIRDistOffset,  //!< in millimeters, ranges from 200 to 1500
00317     ChestIRDistOffset,  //!< in millimeters, ranges from 100 to 900
00318     BAccelOffset, //!< backward acceleration, in @f$m/s^2@f$, negative if sitting on butt (positive for faceplant)
00319     LAccelOffset, //!< acceleration to the robot's left, in @f$m/s^2@f$, negative if lying on robot's left side
00320     DAccelOffset, //!< downward acceleration, in @f$m/s^2@f$, negative if standing up... be careful about the signs on all of these...
00321     PowerRemainOffset, //!< percentage, 0-1
00322     PowerThermoOffset, //!<  degrees Celcius
00323     PowerCapacityOffset, //!< milli-amp hours
00324     PowerVoltageOffset, //!< volts
00325     PowerCurrentOffset //!< milli-amp negative values (maybe positive while charging?)
00326   };
00327 
00328   //! Provides a string name for each sensor
00329   const char* const sensorNames[NumSensors] = {
00330     "NearIRDist","FarIRDist","ChestIRDist",
00331     "BAccel","LAccel","DAccel",
00332     "PowerRemain","PowerThermo","PowerCapacity","PowerVoltage","PowerCurrent"
00333   };
00334 
00335   //@}
00336 
00337 
00338   //! Names for each of the outputs
00339   const char* const outputNames[NumReferenceFrames+1] = {
00340     "LFr:rotor","LFr:elvtr","LFr:knee",
00341     "RFr:rotor","RFr:elvtr","RFr:knee",
00342     "LBk:rotor","LBk:elvtr","LBk:knee",
00343     "RBk:rotor","RBk:elvtr","RBk:knee",
00344     
00345     "NECK:tilt","NECK:pan","NECK:nod",
00346     
00347     "TAIL:tilt",  "TAIL:pan",
00348     
00349     "MOUTH",
00350     
00351     "LED:headC","LED:headW",
00352     "LED:modeR","LED:modeG",
00353     "LED:modeB","LED:wless",
00354     "LED:faceA","LED:faceB","LED:faceC","LED:faceD","LED:faceE","LED:faceF","LED:faceG",
00355     "LED:faceH","LED:faceI","LED:faceJ","LED:faceK","LED:faceL","LED:faceM","LED:faceN",
00356     "LED:bkFrC","LED:bkFrW",
00357     "LED:bkMdC","LED:bkMdW",
00358     "LED:bkRrC","LED:bkRrW",
00359     "LED:ABmod",
00360     
00361     "EAR:left","EAR:right",
00362 
00363     "BaseFrame",
00364     "LFrFootFrame",
00365     "RFrFootFrame",
00366     "LBkFootFrame",
00367     "RBkFootFrame",
00368     "CameraFrame",
00369     "NearIRFrame",
00370     "FarIRFrame",
00371     "ChestIRFrame",
00372     NULL
00373   };
00374   
00375   
00376   //! provides polymorphic robot capability detection/mapping
00377   class ERS7Capabilities : public Capabilities {
00378   public:
00379     //! constructor
00380     ERS7Capabilities()
00381     : Capabilities(TargetName,NumReferenceFrames,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs,NumOutputs)
00382     {
00383       // 2xx button aliases
00384       buttonToIndex["HeadFrBut"]=HeadFrButOffset; // aliased to HeadButOffset
00385       buttonToIndex["BackBut"]=BackButOffset; //aliased to MiddleBackButOffset
00386       // 210 led aliases
00387       frameToIndex["LED:botL"]=BotLLEDOffset; // aliased to face panel...
00388       frameToIndex["LED:botR"]=BotRLEDOffset;
00389       frameToIndex["LED:midL"]=MidLLEDOffset;
00390       frameToIndex["LED:midR"]=MidRLEDOffset;
00391       frameToIndex["LED:topL"]=TopLLEDOffset;
00392       frameToIndex["LED:topR"]=TopRLEDOffset;
00393       frameToIndex["LED:topBr"]=TopBrLEDOffset;
00394       frameToIndex["LED:tlRed"]=TlRedLEDOffset; // aliased to the red back light
00395       frameToIndex["LED:tlBlu"]=TlBluLEDOffset; // aliased to the blue back light
00396       // 220 led aliases
00397       frameToIndex["LED:bkL1"]=TlBluLEDOffset; // aliased to the blue back light
00398       frameToIndex["LED:bkL2"]=TlRedLEDOffset; // aliased to the red back light
00399       // 2xx sensor aliases
00400       sensorToIndex["IRDist"]=IRDistOffset; // aliased to the near IR sensor
00401       frameToIndex["IRFrame"]=IRFrameOffset; // aliased to the near IR sensor
00402       
00403       // the AB switch is a meta-output, don't tell Aperios about it
00404       fakeOutputs.insert(LEDABModeOffset);
00405     }
00406   };
00407   //! allocation declared in RobotInfo.cc
00408   extern const ERS7Capabilities capabilities;
00409   
00410   
00411   //! the joint identifier strings used to refer to specific joints in OPEN-R (but not needed for others)
00412   /*!@showinitializer 
00413    * The offset consts defined in this file correspond to this table and will make life easier
00414    * if you feel the need to reorder things, but they can't be arbitrarily reordered... \n
00415    * In particular, assumptions are made that the pid joints will be in slots 0-numPIDJoints
00416    * and that the fast outputs (ie NOT ears) will be in slots 0-NumFastOutputs\n
00417    * There may be other assumptions not noted here!!!
00418    * @note These entries DON'T correspond to the CPC index numbers (this only lists joints for identifying joints to Aperios, CPCs are for identifying sensors from Aperios */
00419   const char* const PrimitiveName [NumOutputs] = {
00420     "PRM:/r2/c1-Joint2:21",       //!< the left front leg   the rotator
00421     "PRM:/r2/c1/c2-Joint2:22",    //!< the left front leg   the elevator 
00422     "PRM:/r2/c1/c2/c3-Joint2:23", //!< the left front leg   the knee 
00423     "PRM:/r4/c1-Joint2:41",       //!< the right front leg   the rotator
00424     "PRM:/r4/c1/c2-Joint2:42",    //!< the right front leg    the elevator 
00425     "PRM:/r4/c1/c2/c3-Joint2:43", //!< the right front leg   the knee 
00426         
00427     "PRM:/r3/c1-Joint2:31",       //!< the left hind leg   the rotator
00428     "PRM:/r3/c1/c2-Joint2:32",    //!< the left hind leg   the elevator 
00429     "PRM:/r3/c1/c2/c3-Joint2:33", //!< the left hind leg   the knee
00430     "PRM:/r5/c1-Joint2:51",       //!< the right hind leg   the rotator
00431     "PRM:/r5/c1/c2-Joint2:52",    //!< the right hind leg   the elevator 
00432     "PRM:/r5/c1/c2/c3-Joint2:53", //!< the right hind leg   the knee 
00433       
00434     "PRM:/r1/c1-Joint2:11",       //!< the lower neck tilt (12)
00435     "PRM:/r1/c1/c2-Joint2:12",    //!< the neck pan 
00436     "PRM:/r1/c1/c2/c3-Joint2:13", //!< the upper neck tilt (nod)
00437         
00438     "PRM:/r6/c1-Joint2:61",       //!< the tail tilt
00439     "PRM:/r6/c2-Joint2:62",       //!< the tail rotate
00440         
00441     "PRM:/r1/c1/c2/c3/c4-Joint2:14", //!< the mouth (17)
00442         
00443     "PRM:/r1/c1/c2/c3/l1-LED2:l1", //!< Head light (color) (x6, 18)
00444     "PRM:/r1/c1/c2/c3/l2-LED2:l2", //!< Head light (white)
00445     "PRM:/r1/c1/c2/c3/l3-LED2:l3", //!< Red mode indicator
00446     "PRM:/r1/c1/c2/c3/l4-LED2:l4", //!< Green mode indicator
00447     "PRM:/r1/c1/c2/c3/l5-LED2:l5", //!< Blue mode indicator
00448     "PRM:/r1/c1/c2/c3/l6-LED2:l6", //!< wireless light
00449     
00450     "PRM:/r1/c1/c2/c3/la-LED3:la", //!< face lights... (x14, 24-37)
00451     "PRM:/r1/c1/c2/c3/lb-LED3:lb",    
00452     "PRM:/r1/c1/c2/c3/lc-LED3:lc", 
00453     "PRM:/r1/c1/c2/c3/ld-LED3:ld", 
00454     "PRM:/r1/c1/c2/c3/le-LED3:le",    
00455     "PRM:/r1/c1/c2/c3/lf-LED3:lf", 
00456     "PRM:/r1/c1/c2/c3/lg-LED3:lg", 
00457     "PRM:/r1/c1/c2/c3/lh-LED3:lh", 
00458     "PRM:/r1/c1/c2/c3/li-LED3:li", 
00459     "PRM:/r1/c1/c2/c3/lj-LED3:lj", 
00460     "PRM:/r1/c1/c2/c3/lk-LED3:lk", 
00461     "PRM:/r1/c1/c2/c3/ll-LED3:ll", 
00462     "PRM:/r1/c1/c2/c3/lm-LED3:lm", 
00463     "PRM:/r1/c1/c2/c3/ln-LED3:ln", //!< ...last face light (37)
00464 
00465     "PRM:/lu-LED3:lu", //!< front back light (color) (x6, 38)
00466     "PRM:/lv-LED3:lv", //!< front back light (white)
00467     "PRM:/lw-LED3:lw", //!< middle back light (color)
00468     "PRM:/lx-LED3:lx", //!< middle back light (white)
00469     "PRM:/ly-LED3:ly", //!< rear back light (color)
00470     "PRM:/lz-LED3:lz", //!< rear back light (white)
00471     "",  //!< the virtual mode A/B switcher
00472       
00473     "PRM:/r1/c1/c2/c3/e5-Joint4:15", //!< left ear (44)
00474     "PRM:/r1/c1/c2/c3/e6-Joint4:16" //!< right ear
00475   };
00476 
00477   //! use to open speaker connection with the system
00478   const char* const SpeakerLocator="PRM:/s1-Speaker:S1";
00479 
00480   //! use to open camera connection with the system
00481   const char* const CameraLocator="PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1";
00482 
00483   //! This table holds the default PID values for each joint.  see PIDMC
00484   const float DefaultPIDs[NumPIDJoints][3] =
00485     {
00486       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00487       { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00488       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00489       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00490       { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00491       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00492       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00493       { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00494       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00495       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00496       { 0x14/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00497       { 0x1C/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x01/(float)(1<<(16-0xF)) },
00498 
00499       { 0x0A/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x02/(float)(1<<(16-0xF)) },
00500       { 0x08/(float)(1<<(16-0xE)), 0x02/(float)(1<<(16-0x2)), 0x04/(float)(1<<(16-0xF)) },
00501       { 0x08/(float)(1<<(16-0xE)), 0x08/(float)(1<<(16-0x2)), 0x02/(float)(1<<(16-0xF)) }, // 'I' value changed to 0x08 instead of standard 0x04
00502 
00503       { 0x0A/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x04/(float)(1<<(16-0xF)) },
00504       { 0x0A/(float)(1<<(16-0xE)), 0x04/(float)(1<<(16-0x2)), 0x04/(float)(1<<(16-0xF)) },
00505 
00506       { 0x08/(float)(1<<(16-0xE)), 0x00/(float)(1<<(16-0x2)), 0x04/(float)(1<<(16-0xF)) }
00507     };
00508     
00509   //! These will control the shift values given to the system.  see PIDMC
00510   /*! These are modified from the default values to give better range of values to the gains */
00511   const unsigned char DefaultPIDShifts[3] = {0x0E, 0x02-1, 0x0F-3};
00512     
00513   //!These values are Sony's recommended maximum joint velocities, in rad/ms
00514   /*! a value <= 0 means infinite speed (e.g. LEDs)
00515    *  
00516    *  These limits are <b>not</b> enforced by the framework.  They are simply available for you to use as you see fit.
00517    *  HeadPointerMC and PostureMC are primary examples of included classes which do respect these values (although they can be overridden)
00518    *  
00519    *  These values were obtained from the administrators of the Sony OPEN-R BBS */
00520   const float MaxOutputSpeed[NumOutputs] = {
00521     4.86510529f, //Legs LR,FB,REK
00522     5.27962099f,
00523     5.27962099f,
00524     4.86510529f,
00525     5.27962099f,
00526     5.27962099f,
00527     4.86510529f,
00528     5.27962099f,
00529     5.27962099f,
00530     4.86510529f,
00531     5.27962099f,
00532     5.27962099f,
00533   
00534     3.18522588f, //Head TPR
00535     10.0574598f,
00536     5.78140315f,
00537   
00538     15.1625479f, //Tail
00539     15.1625479f,
00540   
00541     10.1447263f, //Mouth
00542     
00543     0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, //LEDs
00544     
00545     0,0                //Ears
00546   };
00547 
00548   #ifndef RAD
00549     //!Just a little macro for converting degrees to radians
00550   #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00551     //!a flag so we undef these after we're done - do you have a cleaner solution?
00552   #define __RI_RAD_FLAG
00553   #endif
00554   
00555   //! This table holds the software limits of each of the outputs, first index is the output offset, second index is MinMaxRange_t (i.e. MinRange or MaxRange)
00556   const float outputRanges[NumOutputs][2] =
00557     {
00558       { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //left front REK
00559       { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //right front REK
00560       { RAD(-130),RAD(115) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //left back REK
00561       { RAD(-130),RAD(115) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //right back REK
00562 
00563       { RAD(-75),RAD(0) },{ RAD(-88),RAD(88) },{ RAD(-15),RAD(45) }, //neck Tilt-pan-nod
00564         
00565       { RAD(5),RAD(60) },{ RAD(-45),RAD(45) }, // tail tp
00566 
00567       { RAD(-55),RAD(-3) }, //mouth
00568 
00569       {0,1},{0,1},{0,1},{0,1},{0,1},{0,1}, //misc LEDs
00570       {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1}, //Face LEDs
00571       {0,1},{0,1},{0,1},{0,1},{0,1},{0,1}, //back LEDs
00572       {0,1}, //virtual mode A/B switcher
00573     
00574       {0,1},{0,1} //ears
00575     };
00576 
00577   //! This table holds the mechanical limits of each of the outputs, first index is the output offset, second index is MinMaxRange_t (i.e. MinRange or MaxRange)
00578   /*! Same as #outputRanges, don't know actual values because they were never specified by Sony */
00579   const float mechanicalLimits[NumOutputs][2] =
00580     {
00581       { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //left front REK
00582       { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //right front REK
00583       { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //left back REK
00584       { RAD(-115),RAD(130) },{ RAD(-10),RAD(88) },{ RAD(-25),RAD(122) }, //right back REK
00585 
00586       { RAD(-75),RAD(0) },{ RAD(-88),RAD(88) },{ RAD(-15),RAD(45) }, //neck Tilt-pan-nod
00587         
00588       { RAD(5),RAD(60) },{ RAD(-45),RAD(45) }, // tail tp
00589 
00590       { RAD(-55),RAD(-3) }, //mouth
00591 
00592       {0,1},{0,1},{0,1},{0,1},{0,1},{0,1}, //misc head LEDs
00593       {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1}, //Face LEDs
00594       {0,1},{0,1},{0,1},{0,1},{0,1},{0,1}, //back LEDs
00595       {0,1}, //virtual mode A/B switcher
00596 
00597       {0,1},{0,1} //ears
00598     };
00599 
00600 #ifdef __RI_RAD_FLAG
00601 #undef RAD
00602 #undef __RI_RAD_FLAG
00603 #endif
00604 
00605 
00606   /*! @name CPC IDs
00607    * Values defined by OPEN-R, used to interface with lower level OPEN-R code to read sensors - values @e don't correspond to order of ERS7Info::PrimitiveName */
00608   const int CPCJointMouth      =  0; //!< Mouth                           
00609   const int CPCSwitchChin      =  1; //!< Chin sensor                     
00610   const int CPCJointNeckNod    =  2; //!< Neck tilt2                      
00611   const int CPCSensorHead      =  3; //!< Head sensor                     
00612   const int CPCSensorNearPSD   =  4; //!< Head distance sensor(near)      
00613   const int CPCSensorFarPSD    =  5; //!< Head distance sensor(far)       
00614   const int CPCJointNeckPan    =  6; //!< Neck pan                        
00615   const int CPCJointNeckTilt   =  7; //!< Neck tilt1                      
00616   const int CPCSwitchLFPaw     =  8; //!< Left fore leg  paw sensor       
00617   const int CPCJointLFKnee     =  9; //!< Left fore legJ3                 
00618   const int CPCJointLFElevator = 10; //!< Left fore legJ2                 
00619   const int CPCJointLFRotator  = 11; //!< Left fore legJ1                 
00620   const int CPCSwitchLHPaw     = 12; //!< Left hind leg  paw sensor       
00621   const int CPCJointLHKnee     = 13; //!< Left hind legJ3                 
00622   const int CPCJointLHElevator = 14; //!< Left hind legJ2                 
00623   const int CPCJointLHRotator  = 15; //!< Left hind legJ1                 
00624   const int CPCSwitchRFPaw     = 16; //!< Right fore leg  paw sensor      
00625   const int CPCJointRFKnee     = 17; //!< Right fore legJ3                
00626   const int CPCJointRFElevator = 18; //!< Right fore legJ2                
00627   const int CPCJointRFRotator  = 19; //!< Right fore legJ1                
00628   const int CPCSwitchRHPaw     = 20; //!< Right hind leg  paw sensor      
00629   const int CPCJointRHKnee     = 21; //!< Right hind legJ3                
00630   const int CPCJointRHElevator = 22; //!< Right hind legJ2                
00631   const int CPCJointRHRotator  = 23; //!< Right hind legJ1                
00632   const int CPCJointTailTilt   = 24; //!< Tail tilt                       
00633   const int CPCJointTailPan    = 25; //!< Tail pan                        
00634   const int CPCSensorAccelFB   = 26; //!< Acceleration sensor(front-back) 
00635   const int CPCSensorAccelLR   = 27; //!< Acceleration sensor(right-left) 
00636   const int CPCSensorAccelUD   = 28; //!< Acceleration sensor(up-down)    
00637   const int CPCSensorChestPSD  = 29; //!< Chest distance sensor           
00638   const int CPCSwitchWireless  = 30; //!< Wireless LAN switch             
00639   const int CPCSensorBackRear  = 31; //!< Back sensor(rear)               
00640   const int CPCSensorBackMiddle= 32; //!< Back sensor(middle)             
00641   const int CPCSensorBackFront = 33; //!< Back sensor(front)              
00642   //@}
00643 
00644 }
00645   
00646 /*! @file
00647  * @brief Defines RobotInfo namespace for ERS-7 models, gives some information about the robot's capabilities, such as joint counts, offsets, names and PID values
00648  * @author ejt (Creator)
00649  */
00650 
00651 #endif

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