Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MantisLegInfo.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef INCLUDED_MantisLegInfo_h
00003 #define INCLUDED_MantisLegInfo_h
00004 
00005 #include <cmath>
00006 #include <stdlib.h>
00007 #include "CommonInfo.h"
00008 using namespace RobotInfo;
00009 
00010 // see http://tekkotsu.org/porting.html#configuration for more information on TGT_HAS_* flags
00011 #if defined(TGT_MANTISLEG)
00012 #  define TGT_IS_MANTISLEG
00013 #  define TGT_HAS_LEGS 1
00014 #  define TGT_HAS_LEDS 4
00015 #  define TGT_HAS_POWER_STATUS
00016 #endif
00017 
00018 //! Contains information about an Mantis Leg robot, such as number of joints, LEDs, etc.
00019 namespace MantisLegInfo {
00020 
00021   // *******************************
00022   //       ROBOT CONFIGURATION
00023   // *******************************
00024 
00025   extern const char* const TargetName; //!< the name of the model, to be used for logging and remote GUIs
00026 
00027   const unsigned int FrameTime  = 32;    //!< time between frames in the motion system (milliseconds)
00028   const unsigned int NumFrames  = 1;     //!< the number of frames per buffer (don't forget also double buffered)
00029   const unsigned int SoundBufferTime = 32; //!< the number of milliseconds per sound buffer... I'm not sure if this can be changed
00030   
00031   //!@name Output Types Information
00032   const unsigned NumWheels      =  0;
00033   
00034   const unsigned JointsPerArm   =  0;
00035   const unsigned NumArms        =  0;
00036   const unsigned NumArmJoints   =  JointsPerArm*NumArms;
00037   
00038   const unsigned JointsPerLeg   =  4; //!< The number of joints per leg
00039   const unsigned NumLegs        =  1; //!< The number of legs
00040   const unsigned NumLegJoints   =  JointsPerLeg*NumLegs; //!< the TOTAL number of joints on ALL legs
00041 
00042   const unsigned NumHeadJoints  =  0; //!< The number of joints in the pantilt
00043   const unsigned NumTailJoints  =  0; //!< The number of joints assigned to the tail
00044   const unsigned NumMouthJoints =  0; //!< the number of joints that control the mouth
00045   const unsigned NumEarJoints   =  0; //!< The number of joints which control the ears (NOT per ear, is total)
00046   const unsigned NumButtons     =  3; //!< the number of buttons that are available
00047   const unsigned NumSensors     =  11; //!< the number of sensors available
00048   const unsigned NumFacePanelLEDs = 0; //!< The number of face panel LEDs
00049 
00050   const unsigned NumPIDJoints   = NumArmJoints + NumLegJoints + NumHeadJoints + NumTailJoints + NumMouthJoints; //!< servo pins
00051   const unsigned NumLEDs        = NumPIDJoints; //!< There's an LED on every dynamixel, but can't see most of them, so save some computational resources and only expose the visible ones
00052   
00053   const unsigned NumOutputs     = NumWheels + NumPIDJoints + NumLEDs; //!< the total number of outputs
00054   const unsigned NumReferenceFrames = NumOutputs + NumLegs + NumArms + 1; //!< for the base, feet, gripper, camera, and IR distance rangefinder
00055 
00056   using namespace Camera75DOF;
00057   
00058   const float BallOfFootRadius = 0; //!< radius of the ball of the foot
00059 
00060 
00061   // *******************************
00062   //         OUTPUT OFFSETS
00063   // *******************************
00064 
00065   //!Corresponds to entries in ERS7Info::PrimitiveName, defined at the end of this file
00066   //!@name Output Offsets
00067 
00068   const unsigned PIDJointOffset = 0; //!< The beginning of the PID Joints
00069   const unsigned LegOffset    = PIDJointOffset; //!< the offset of the beginning of the regular leg joints (after the 1 rotator joint for the right front leg):  #NumLegs of #JointsPerLeg each, in #LegOrder_t order; see #LegOffset_t
00070   const unsigned LEDOffset    = PIDJointOffset + NumPIDJoints; //!< the offset of LEDs in WorldState::outputs and MotionCommand functions, see LedOffset_t for specific offsets
00071   const unsigned BaseFrameOffset  = NumOutputs;   //!< Use with kinematics to refer to base reference frame
00072   const unsigned FootFrameOffset  = BaseFrameOffset+1;  //!< Use with kinematics to refer to feet reference frames (add appropriate LegOrder_t to specify which paw)
00073     const unsigned GripperFrameOffset = FootFrameOffset+NumLegs; //!< Use with kinematics to refer to gripper reference frame
00074   const unsigned CameraFrameOffset = GripperFrameOffset+NumArms; //!< Use with kinematics to refer to camera reference frame
00075     
00076     const unsigned LeftIRFrameOffset = CameraFrameOffset+1; //!< Use with kinematics to refer to left IR distance rangefinder reference frame
00077   const unsigned CenterIRFrameOffset = LeftIRFrameOffset+1; //!< Use with kinematics to refer to center IR distance rangefinder reference frame
00078   const unsigned IRFrameOffset = CenterIRFrameOffset; //!< alias for CenterIRFrameOffset
00079   const unsigned RightIRFrameOffset = CenterIRFrameOffset+1; //!< Use with kinematics to refer to right IR distance rangefinder reference frame
00080 
00081   
00082   //! the ordering of legs
00083   enum LegOrder_t {
00084     MdLegOrder = 0, //!< middle leg
00085   };
00086   
00087   //! The offsets within appendages (the legs)  Note that the ordering matches the actual physical ordering of joints on the appendage
00088   enum SEKOffset_t {
00089     SweepOffset = 0,  //!< moves leg forward or backward along body
00090     ElevatorOffset,   //!< moves leg toward or away from body
00091     RotorOffset,    //!< rotates leg
00092     KneeOffset      //!< moves knee
00093   };
00094   
00095   //! The offsets of appendages with tilt (elevation), pan (heading), and roll or nod joints (i.e. head/wrist)
00096   enum TPROffset_t {
00097     PanOffset = 0,    //!< pan/yaw/heading (horizontal)
00098     TiltOffset,     //!< tilt/pitch/elevation (vertical)
00099     NodOffset = TiltOffset, //!< replicated tilt (could be left undefined instead...)
00100     RollOffset      //!< spin/twist final axis
00101   };
00102   
00103   //! The offsets of the individual legs, add #REKOffset_t value to access specific joint
00104   /*! @hideinitializer */
00105   enum LegOffset_t {
00106     MdLegOffset = LegOffset+MdLegOrder*JointsPerLeg, //!< beginning of right front leg's joints
00107   };
00108     
00109     //! The offsets of the individual LEDs
00110   /*! @hideinitializer */
00111   enum LEDOffset_t {
00112     PowerRedLEDOffset=LEDOffset,
00113     PowerGreenLEDOffset,
00114     PlayLEDOffset,
00115     AdvanceLEDOffset
00116   };
00117     
00118     
00119     const LEDOffset_t RedLEDOffset = PowerRedLEDOffset;
00120   const LEDOffset_t BlueLEDOffset = AdvanceLEDOffset; 
00121   const LEDOffset_t GreenLEDOffset = PlayLEDOffset;
00122   const LEDOffset_t YellowLEDOffset = AdvanceLEDOffset;
00123     
00124   typedef unsigned int LEDBitMask_t; //!< So you can be clear when you're refering to a LED bitmask
00125     
00126   const LEDBitMask_t BlueLEDMask = (1<<(AdvanceLEDOffset-LEDOffset)) | (1<<(PowerRedLEDOffset-LEDOffset));
00127   const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00128   const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00129   const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00130     
00131   const LEDBitMask_t PowerRedLEDMask = 1<<(PowerRedLEDOffset-LEDOffset); //!< mask corresponding to BlueLEDOffset
00132   const LEDBitMask_t PowerGreenLEDMask = 1<<(PowerGreenLEDOffset-LEDOffset); //!< mask corresponding to GreenLEDOffset
00133   const LEDBitMask_t PlayLEDMask = 1<<(PlayLEDOffset-LEDOffset); //!< mask corresponding to YellowLEDOffset
00134   const LEDBitMask_t AdvanceLEDMask = 1<<(AdvanceLEDOffset-LEDOffset); //!< mask corresponding to RedLEDOffset
00135     
00136   //! LEDs for the face panel (all FaceLEDPanelMask<<(0:NumFacePanelLEDs-1) entries)
00137   const LEDBitMask_t FaceLEDMask = 0;
00138   //! selects all of the leds
00139   const LEDBitMask_t AllLEDMask  = (LEDBitMask_t)~0;
00140   //@}
00141     
00142     
00143     //! Offset needed so that the centroid of the robot is correct related to the bounding box
00144   const fmat::Column<3> AgentBoundingBoxBaseFrameOffset = fmat::pack(400,70,350);
00145     
00146   //! Half of the length, width, and height of the robot.
00147   const fmat::Column<3> AgentBoundingBoxHalfDims = fmat::pack(100,100,100);
00148   //@}
00149     
00150 
00151   // *******************************
00152   //          INPUT OFFSETS
00153   // *******************************
00154 
00155 
00156   //! The order in which inputs should be stored
00157   //!@name Input Offsets
00158 
00159   //! holds offsets to different buttons in WorldState::buttons[]
00160   /*! Should be a straight mapping to the ButtonSourceIDs
00161    *
00162    *  Note that the chest (power) button is not a normal button.  It kills
00163    *  power to the motors at a hardware level, and isn't sensed in the
00164    *  normal way.  If you want to know when it is pressed (and you are
00165    *  about to shut down) see PowerSrcID::PauseSID.
00166    *
00167    *  @see WorldState::buttons @see ButtonSourceID_t
00168    * @hideinitializer */
00169   enum ButtonOffset_t { GreenButOffset, RedButOffset, YellowButOffset };
00170 
00171   //! Provides a string name for each button
00172   const char* const buttonNames[NumButtons+1] = { "GreenBut", "RedBut", "YellowBut", NULL };
00173 
00174   //! holds offset to different sensor values in WorldState::sensors[]
00175   /*! @see WorldState::sensors[] */
00176   enum SensorOffset_t {
00177     LeftIRDistOffset,
00178     CenterIRDistOffset,
00179     IRDistOffset = CenterIRDistOffset,
00180     RightIRDistOffset,
00181     LeftLuminosityOffset,
00182     CenterLuminosityOffset,
00183     RightLuminosityOffset,
00184     MicVolumeOffset,
00185     MicSpikeCountOffset,
00186     PowerRemainOffset, //! estimate of power capacity as percentage, 0-1
00187     PowerThermoOffset, //!< degrees Celcius
00188     PowerVoltageOffset, //!< volts
00189   };
00190 
00191   //! Provides a string name for each sensor
00192   const char* const sensorNames[NumSensors+1] = {
00193     "LeftIRDist", "CenterIRDist", "RightIRDist",
00194     "LeftLuminosity", "CenterLuminosity", "RightLuminosity",
00195     "MicVolume", "MicSpikeCount",
00196     "PowerRemain", "PowerThermo", "PowerVoltage", NULL
00197   };
00198 
00199   //@}
00200   
00201   
00202   //! Names for each of the outputs
00203   const char* const outputNames[NumReferenceFrames+1] = {
00204     // servos:
00205     "sweep","elevator","rotor","knee",
00206     
00207         // LEDs
00208     "LED:sweep","LED:elevator",
00209     "LED:rotor","LED:knee",
00210         
00211         // Reference frames
00212         "BaseFrame", "FootFrame",
00213       
00214     NULL
00215   };
00216   
00217   //! provides polymorphic robot capability detection/mapping
00218   class MantisLegCapabilities : public Capabilities {
00219   public:
00220     //! constructor
00221     MantisLegCapabilities()
00222     : Capabilities(TargetName,NumReferenceFrames,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs,NumOutputs)
00223     {
00224       frameToIndex["IRFrame"] = IRFrameOffset;  // aliased to the center IR sensor
00225       sensorToIndex["IRDist"] = IRDistOffset;   // aliased to the center IR sensor
00226     }
00227   };
00228     
00229   //! allocation declared in RobotInfo.cc
00230   extern const MantisLegCapabilities capabilities;
00231   
00232   
00233   //! offsets into DefaultPIDs, since Dynamixel servos don't actually use PID control, but a different set of parameters
00234   enum ServoParam_t {
00235     DYNAMIXEL_SLOPE = 0,  //!< compliance slope, the proportional control (P in PID)
00236     DYNAMIXEL_PUNCH,    //!< punch, a constant added to the slope once error exceeds compliance margin
00237     DYNAMIXEL_MARGIN    //!< compliance margin, the amount of error to tolerate before triggering a torque response
00238   };
00239   
00240   //! Dynamixel servos don't use PID control.  Instead, these values indicate compliance slope (P), punch (add to P*error), compliance margin (min error to start applying torque) (see ServoParam_t)
00241   /*! I believe the torque calculation goes something like: torque = (error<compliance) ? 0 : punch + P*error
00242    *  Dynamixel servos allow different values to be supplied for CW vs. CCW motion, but we just use the same value for each */
00243   const float DefaultPIDs[NumPIDJoints][3] = {
00244     {32,32,0},{32,32,0},
00245         {32,32,0},{32,32,0}
00246   };
00247   
00248   //!These values are our recommended maximum joint velocities, in rad/ms
00249   /*! a value <= 0 means infinite speed (e.g. LEDs)
00250    *  
00251    *  These limits are <b>not</b> enforced by the framework.  They are simply available for you to use as you see fit.
00252    *  HeadPointerMC and PostureMC are primary examples of included classes which do respect these values (although they can be overridden)
00253    *  
00254    *  These values were obtained from the administrators of the Sony OPEN-R BBS */
00255   const float MaxOutputSpeed[NumOutputs] = {
00256     // servos
00257     0.4f, 0.4f, 0.4f, 0.4f, 
00258     // leds
00259     0,0,0,0 
00260   };
00261 
00262   #ifndef RAD
00263     //!Just a little macro for converting degrees to radians
00264   #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00265     //!a flag so we undef these after we're done - do you have a cleaner solution?
00266   #define __RI_RAD_FLAG
00267   #endif
00268   
00269   //! 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)
00270   const float outputRanges[NumOutputs][2] = {
00271     // servos
00272     {RAD(-100),RAD(100)}, 
00273         {RAD(-100),RAD(100)},
00274     {RAD(-100),RAD(100)},
00275         {RAD(-100),RAD(100)},   
00276     
00277         // LED
00278     {0,1},{0,1},
00279         {0,1},{0,1},
00280   };
00281 
00282   //! 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)
00283   /*! Same as #outputRanges, don't know actual values because they were never specified by Sony */
00284   const float mechanicalLimits[NumOutputs][2] = {
00285     // servos
00286     {RAD(-100),RAD(100)}, 
00287         {RAD(-100),RAD(100)},
00288     {RAD(-100),RAD(100)},
00289         {RAD(-100),RAD(100)},
00290     // LED
00291     {0,1},{0,1},
00292         {0,1},{0,1},
00293   };
00294 
00295 #ifdef __RI_RAD_FLAG
00296 #undef RAD
00297 #undef __RI_RAD_FLAG
00298 #endif
00299 }
00300 
00301 /*! @file
00302  * @brief Defines some capabilities of the Mantis leg robots
00303  * @author ejt (Creator)
00304  */
00305 
00306 #endif

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