00001 #ifndef INCLUDED_MantisInfo_h
00002 #define INCLUDED_MantisInfo_h
00003
00004 #include <cmath>
00005 #include <stdlib.h>
00006 #include "CommonInfo.h"
00007 using namespace RobotInfo;
00008
00009
00010 #if defined(TGT_MANTIS)
00011 # define TGT_IS_MANTIS
00012 # define TGT_HAS_CAMERA 1
00013 # define TGT_HAS_LEGS 6
00014 # define TGT_HAS_LEDS 16
00015 # define TGT_HAS_HEAD 1
00016 # define TGT_HAS_POWER_STATUS
00017 # define TGT_HAS_KINECT 1
00018 # define TGT_HAS_GRIPPER 2
00019 #endif
00020
00021
00022 namespace MantisInfo {
00023
00024
00025
00026
00027
00028 extern const char* const TargetName;
00029
00030 const unsigned int FrameTime=32;
00031 const unsigned int NumFrames=1;
00032 const unsigned int SoundBufferTime=32;
00033
00034
00035 const unsigned NumWheels = 0;
00036
00037 const unsigned FingerJointsPerArm = 0;
00038 const unsigned JointsPerArm = 0;
00039 const unsigned NumArms = 0;
00040 const unsigned NumArmJoints = JointsPerArm*NumArms;
00041
00042 const unsigned JointsPerFrLeg = 7;
00043 const unsigned JointsPerPosLeg = 4;
00044 const unsigned NumFrLegs = 2;
00045 const unsigned NumGrippers = NumFrLegs;
00046 const unsigned NumPosLegs = 4;
00047 const unsigned NumLegs = NumFrLegs + NumPosLegs;
00048 const unsigned NumFrLegJoints = JointsPerFrLeg*NumFrLegs;
00049 const unsigned NumPosLegJoints = JointsPerPosLeg*NumPosLegs;
00050 const unsigned NumLegJoints = NumFrLegJoints + NumPosLegJoints;
00051
00052 const unsigned NumHeadJoints = 3;
00053 const unsigned NumTailJoints = 0;
00054 const unsigned NumMouthJoints = 0;
00055 const unsigned NumEarJoints = 0;
00056 const unsigned NumButtons = 3;
00057 const unsigned NumSensors = 11;
00058 const unsigned NumFacePanelLEDs = 0;
00059
00060 const unsigned NumPIDJoints = NumArmJoints + 1 + NumLegJoints + NumHeadJoints + NumTailJoints + NumMouthJoints;
00061 const unsigned NumLEDs = NumPIDJoints;
00062
00063 const unsigned NumOutputs = NumWheels + NumPIDJoints + NumLEDs;
00064 const unsigned NumReferenceFrames = NumOutputs + NumLegs + NumArms + NumGrippers + 1 + 1;
00065
00066 using namespace Camera75DOF;
00067
00068 const float BallOfFootRadius = 0;
00069 const unsigned FrontLegExtra = JointsPerFrLeg -1;
00070
00071
00072
00073
00074
00075
00076
00077
00078 const unsigned PIDJointOffset = 0;
00079 const unsigned LegOffset = PIDJointOffset;
00080 const unsigned ThoraxJointOffset = LegOffset+NumLegJoints;
00081 const unsigned HeadOffset = ThoraxJointOffset + 1;
00082
00083 const unsigned LEDOffset = PIDJointOffset + NumPIDJoints;
00084
00085 const unsigned BaseFrameOffset = NumOutputs;
00086 const unsigned FootFrameOffset = BaseFrameOffset + 1;
00087 const unsigned GripperFrameOffset = FootFrameOffset+NumLegs;
00088 const unsigned CameraFrameOffset = GripperFrameOffset + NumGrippers;
00089
00090
00091 enum LegOrder_t {
00092 LFrLegOrder = 0,
00093 RFrLegOrder,
00094 LMdLegOrder,
00095 RMdLegOrder,
00096 LBkLegOrder,
00097 RBkLegOrder
00098 };
00099
00100
00101 enum FrLegOffset_t {
00102 FrSweepOffset = 0,
00103 FrElevatorOffset,
00104 FrTwist1Offset,
00105 FrElbowOffset,
00106 FrTwist2Offset,
00107 FrWristOffset,
00108 FrGripperOffset
00109 };
00110
00111
00112
00113 enum PosLegOffset_t {
00114 PosSweepOffset = 0,
00115 PosRotorOffset,
00116 PosElevatorOffset,
00117 PosKneeOffset
00118 };
00119
00120
00121 enum TPROffset_t {
00122 PanOffset = 0,
00123 TiltOffset,
00124
00125 RollOffset
00126 };
00127
00128
00129
00130 enum LegOffset_t {
00131 LFrLegOffset = LegOffset+LFrLegOrder*JointsPerFrLeg,
00132 RFrLegOffset = LegOffset+RFrLegOrder*JointsPerFrLeg,
00133 LMdLegOffset = LegOffset+LMdLegOrder*JointsPerPosLeg + FrontLegExtra,
00134 RMdLegOffset = LegOffset+RMdLegOrder*JointsPerPosLeg + FrontLegExtra,
00135 LBkLegOffset = LegOffset+LBkLegOrder*JointsPerPosLeg + FrontLegExtra,
00136 RBkLegOffset = LegOffset+RBkLegOrder*JointsPerPosLeg + FrontLegExtra,
00137 };
00138
00139
00140 enum HeadOffset_t {
00141 HeadPanOffset = HeadOffset,
00142 HeadTiltOffset,
00143 HeadRollOffset,
00144 };
00145
00146
00147
00148 enum LEDOffset_t {
00149 PowerRedLEDOffset=LEDOffset,
00150 PowerGreenLEDOffset,
00151 PlayLEDOffset,
00152 AdvanceLEDOffset
00153 };
00154
00155 const LEDOffset_t RedLEDOffset = PowerRedLEDOffset;
00156 const LEDOffset_t BlueLEDOffset = AdvanceLEDOffset;
00157 const LEDOffset_t GreenLEDOffset = PlayLEDOffset;
00158 const LEDOffset_t YellowLEDOffset = AdvanceLEDOffset;
00159
00160 typedef unsigned int LEDBitMask_t;
00161
00162 const LEDBitMask_t BlueLEDMask = (1<<(AdvanceLEDOffset-LEDOffset)) | (1<<(PowerRedLEDOffset-LEDOffset));
00163 const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset);
00164 const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset);
00165 const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset);
00166
00167 const LEDBitMask_t PowerRedLEDMask = 1<<(PowerRedLEDOffset-LEDOffset);
00168 const LEDBitMask_t PowerGreenLEDMask = 1<<(PowerGreenLEDOffset-LEDOffset);
00169 const LEDBitMask_t PlayLEDMask = 1<<(PlayLEDOffset-LEDOffset);
00170 const LEDBitMask_t AdvanceLEDMask = 1<<(AdvanceLEDOffset-LEDOffset);
00171
00172
00173 const LEDBitMask_t FaceLEDMask = 0;
00174
00175 const LEDBitMask_t AllLEDMask = (LEDBitMask_t)~0;
00176
00177
00178
00179
00180 const fmat::Column<3> AgentBoundingBoxBaseFrameOffset = fmat::pack(400,70,350);
00181
00182
00183 const fmat::Column<3> AgentBoundingBoxHalfDims = fmat::pack(100,100,100);
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205 enum ButtonOffset_t { GreenButOffset, RedButOffset, YellowButOffset };
00206
00207
00208 const char* const buttonNames[NumButtons+1] = { "GreenBut", "RedBut", "YellowBut", NULL };
00209
00210
00211
00212 enum SensorOffset_t {
00213 LeftIRDistOffset,
00214 CenterIRDistOffset,
00215 IRDistOffset = CenterIRDistOffset,
00216 RightIRDistOffset,
00217 LeftLuminosityOffset,
00218 CenterLuminosityOffset,
00219 RightLuminosityOffset,
00220 MicVolumeOffset,
00221 MicSpikeCountOffset,
00222 PowerRemainOffset,
00223 PowerThermoOffset,
00224 PowerVoltageOffset,
00225 };
00226
00227
00228 const char* const sensorNames[NumSensors+1] = {
00229 "LeftIRDist", "CenterIRDist", "RightIRDist",
00230 "LeftLuminosity", "CenterLuminosity", "RightLuminosity",
00231 "MicVolume", "MicSpikeCount",
00232 "PowerRemain", "PowerThermo", "PowerVoltage", NULL
00233 };
00234
00235
00236 const char* const outputNames[NumReferenceFrames+1] = {
00237
00238 "LFr:sweep","LFr:elvtr","LFr:twist1","LFr:elbow","LFr:twist2","LFr:wrist","LFr:Gripper",
00239 "RFr:sweep","RFr:elvtr","RFr:twist1","RFr:elbow","RFr:twist2","RFr:wrist","RFr:Gripper",
00240 "LMd:sweep","LMd:rotor","LMd:elvtr","LMd:knee",
00241 "RMd:sweep","RMd:rotor","RMd:elvtr","RMd:knee",
00242 "LBk:sweep","LBk:rotor","LBk:elvtr","LBk:knee",
00243 "RBk:sweep","RBk:rotor","RBk:elvtr","RBk:knee",
00244 "Thorax",
00245 "NECK:pan","NECK:tilt","NECK:roll",
00246
00247
00248 "LED:LFr:sweep","LED:LFr:elvtr","LED:LFr:twist1","LED:LFr:elbow","LED:LFr:twist2","LED:LFr:wrist","LED:LFr:Gripper",
00249 "LED:RFr:sweep","LED:RFr:elvtr","LED:RFr:twist1","LED:RFr:elbow","LED:RFr:twist2","LED:RFr:wrist","LED:RFr:Gripper",
00250 "LED:LMd:sweep","LED:LMd:rotor","LED:LMd:elvtr","LED:LMd:knee",
00251 "LED:RMd:sweep","LED:RMd:rotor","LED:RMd:elvtr","LED:RMd:knee",
00252 "LED:LBk:sweep","LED:LBk:rotor","LED:LBk:elvtr","LED:LBk:knee",
00253 "LED:RBk:sweep","LED:RBk:rotor","LED:RBk:elvtr","LED:RBk:knee",
00254 "LED:Thorax",
00255 "LED:NECK:pan","LED:NECK:tilt","LED:NECK:roll",
00256
00257
00258 "BaseFrame","LFrFootFrame","RFrFootFrame","LMdFootFrame","RMdFootFrame","LBkFootFrame","RBkFootFrame",
00259 "LFrGripperFrame", "RFrGripperFrame", "CameraFrame",
00260 NULL
00261 };
00262
00263
00264 class MantisCapabilities : public Capabilities {
00265 public:
00266
00267 MantisCapabilities() : Capabilities(TargetName,NumReferenceFrames,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,
00268 PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs,NumOutputs) {
00269 frameToIndex["NECK:nod"] = HeadRollOffset;
00270 }
00271 };
00272
00273
00274 extern const MantisCapabilities capabilities;
00275
00276
00277
00278 enum ServoParam_t {
00279 DYNAMIXEL_SLOPE = 0,
00280 DYNAMIXEL_PUNCH,
00281 DYNAMIXEL_MARGIN
00282 };
00283
00284
00285
00286
00287 const float DefaultPIDs[NumPIDJoints][3] = {
00288 {32,32,0},{32,32,0},{32,32,0},{32,32,0},{32,32,0},{32,32,0},{32,32,0},
00289 {32,32,0},{32,32,0},{32,32,0},{32,32,0},{32,32,0},{32,32,0},{32,32,0},
00290 {32,32,0},{32,32,0},{32,32,0},{32,32,0},
00291 {32,32,0},{32,32,0},{32,32,0},{32,32,0},
00292 {32,32,0},{32,32,0},{32,32,0},{32,32,0},
00293 {32,32,0},{32,32,0},{32,32,0},{32,32,0},
00294 {32,32,0},
00295 {32,32,0},{32,32,0},{32,32,0}
00296 };
00297
00298
00299
00300
00301
00302
00303
00304
00305 const float MaxOutputSpeed[NumOutputs] = {
00306
00307 0,0,0,0,0,0,0,
00308 0,0,0,0,0,0,0,
00309 0,0,0,0,
00310 0,0,0,0,
00311 0,0,0,0,
00312 0,0,0,0,
00313 0,
00314 0,0,0,
00315
00316 0,0,0,0,0,0,0,
00317 0,0,0,0,0,0,0,
00318 0,0,0,0,
00319 0,0,0,0,
00320 0,0,0,0,
00321 0,0,0,0,
00322 0,
00323 0,0,0
00324 };
00325
00326 #ifndef RAD
00327
00328 #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00329
00330 #define __RI_RAD_FLAG
00331 #endif
00332
00333
00334 const float outputRanges[NumOutputs][2] = {
00335
00336 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00337 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00338 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00339 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00340 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00341 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00342 {RAD(-100),RAD(100)},
00343 {RAD(-100),RAD(100)},{RAD(-100),RAD(100)},{RAD(-100),RAD(100)},
00344
00345
00346 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00347 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00348 {0,1},{0,1},{0,1},{0,1},
00349 {0,1},{0,1},{0,1},{0,1},
00350 {0,1},{0,1},{0,1},{0,1},
00351 {0,1},{0,1},{0,1},{0,1},
00352 {0,1},
00353 {0,1},{0,1},{0,1}
00354 };
00355
00356
00357
00358 const float mechanicalLimits[NumOutputs][2] = {
00359
00360 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00361 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},
00362 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},{RAD(-100),RAD(100)},
00363 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},{RAD(-100),RAD(100)},
00364 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},{RAD(-100),RAD(100)},
00365 {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)}, {RAD(-100),RAD(100)},{RAD(-100),RAD(100)},
00366 {RAD(-100),RAD(100)},
00367 {RAD(-100),RAD(100)},{RAD(-100),RAD(100)},{RAD(-100),RAD(100)},
00368
00369 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00370 {0,1},{0,1},{0,1},{0,1},{0,1},{0,1},{0,1},
00371 {0,1},{0,1},{0,1},{0,1},
00372 {0,1},{0,1},{0,1},{0,1},
00373 {0,1},{0,1},{0,1},{0,1},
00374 {0,1},{0,1},{0,1},{0,1},
00375 {0,1},
00376 {0,1},{0,1},{0,1}
00377 };
00378
00379 #ifdef __RI_RAD_FLAG
00380 #undef RAD
00381 #undef __RI_RAD_FLAG
00382 #endif
00383 }
00384
00385
00386
00387
00388
00389
00390
00391 #endif