00001
00002 #ifndef INCLUDED_ChiaraInfo_h
00003 #define INCLUDED_ChiaraInfo_h
00004
00005 #include <cmath>
00006 #include <stdlib.h>
00007 #include "CommonInfo.h"
00008 using namespace RobotInfo;
00009
00010
00011 #if defined(TGT_CHIARA)
00012 # define TGT_IS_BIOLOID
00013 # define TGT_IS_CHIARA
00014 # define TGT_HAS_CAMERA 1
00015 # define TGT_HAS_LEDS 11
00016 # define TGT_HAS_BUTTONS 3
00017 # define TGT_HAS_LEGS 6
00018 # define TGT_HAS_SEK_LEGS 6
00019 # define TGT_HAS_ARMS 1
00020 # define TGT_HAS_HEAD 1
00021 # define TGT_HAS_POWER_STATUS
00022 # define TGT_HAS_IR_DISTANCE 3
00023 #endif
00024
00025
00026 namespace ChiaraInfo {
00027
00028
00029
00030
00031
00032 extern const char* const TargetName;
00033
00034 const unsigned int FrameTime=32;
00035 const unsigned int NumFrames=1;
00036 const unsigned int SoundBufferTime=32;
00037
00038
00039 const unsigned NumWheels = 0;
00040
00041 const unsigned FingerJointsPerArm = 2;
00042 const unsigned JointsPerArm = 6;
00043 const unsigned NumArms = 1;
00044 const unsigned NumArmJoints = JointsPerArm*NumArms;
00045
00046 const unsigned JointsPerLeg = 3;
00047 const unsigned NumLegs = 6;
00048 const unsigned NumLegJoints = JointsPerLeg*NumLegs;
00049 const unsigned NumHeadJoints = 2;
00050 const unsigned NumTailJoints = 0;
00051 const unsigned NumMouthJoints = 0;
00052 const unsigned NumEarJoints = 0;
00053 const unsigned NumButtons = 3;
00054 const unsigned NumSensors = 11;
00055 const unsigned NumFacePanelLEDs = 0;
00056
00057 const unsigned NumLEDs = NumLegs + 1 + 4;
00058 const unsigned NumPIDJoints = NumArmJoints + 1 + NumLegJoints + NumHeadJoints+NumTailJoints+NumMouthJoints;;
00059
00060 const unsigned NumOutputs = NumWheels + NumPIDJoints + NumLEDs;
00061 const unsigned NumReferenceFrames = NumOutputs + 1 + NumLegs + NumArms + 1 + 3;
00062
00063 using namespace Camera75DOF;
00064
00065 const float BallOfFootRadius=0;
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 const unsigned PIDJointOffset = 0;
00078 const unsigned RFrRotatorOffset = PIDJointOffset;
00079 const unsigned LegOffset = RFrRotatorOffset+1;
00080 const unsigned ArmOffset = LegOffset+NumLegJoints;
00081 const unsigned HeadOffset = ArmOffset+NumArmJoints;
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+NumArms;
00089
00090 const unsigned LeftIRFrameOffset = CameraFrameOffset+1;
00091 const unsigned CenterIRFrameOffset = LeftIRFrameOffset+1;
00092 const unsigned IRFrameOffset = CenterIRFrameOffset;
00093 const unsigned RightIRFrameOffset = CenterIRFrameOffset+1;
00094
00095
00096 enum LegOrder_t {
00097 RFrLegOrder = 0,
00098 LFrLegOrder,
00099 RMdLegOrder,
00100 LMdLegOrder,
00101 RBkLegOrder,
00102 LBkLegOrder
00103 };
00104
00105
00106 enum SEKOffset_t {
00107 SweepOffset=0,
00108 ElevatorOffset,
00109 KneeOffset
00110 };
00111
00112
00113 enum TPROffset_t {
00114 PanOffset=0,
00115 TiltOffset,
00116 NodOffset = TiltOffset,
00117 RollOffset
00118 };
00119
00120
00121
00122 enum LegOffset_t {
00123 RFrLegOffset = LegOffset+RFrLegOrder*JointsPerLeg,
00124 LFrLegOffset = LegOffset+LFrLegOrder*JointsPerLeg,
00125 RMdLegOffset = LegOffset+RMdLegOrder*JointsPerLeg,
00126 LMdLegOffset = LegOffset+LMdLegOrder*JointsPerLeg,
00127 RBkLegOffset = LegOffset+RBkLegOrder*JointsPerLeg,
00128 LBkLegOffset = LegOffset+LBkLegOrder*JointsPerLeg,
00129 };
00130
00131
00132 enum ArmOffset_t {
00133 ArmShoulderOffset=ArmOffset,
00134 ArmElbowOffset,
00135 WristOffset,
00136 WristYawOffset = WristOffset,
00137 WristPitchOffset,
00138 WristRollOffset,
00139 GripperOffset
00140 };
00141
00142
00143 enum HeadOffset_t {
00144 HeadPanOffset = HeadOffset,
00145 HeadTiltOffset,
00146 };
00147
00148
00149
00150 enum LEDOffset_t {
00151 BlueLEDOffset = LEDOffset,
00152 GreenLEDOffset,
00153 YellowLEDOffset,
00154 RedLEDOffset,
00155 RFrKneeLEDOffset,
00156 LFrKneeLEDOffset,
00157 RMdKneeLEDOffset,
00158 LMdKneeLEDOffset,
00159 RBkKneeLEDOffset,
00160 LBkKneeLEDOffset,
00161 HeadLEDOffset
00162 };
00163
00164 typedef unsigned int LEDBitMask_t;
00165
00166 const LEDBitMask_t BlueLEDMask = 1<<(BlueLEDOffset-LEDOffset);
00167 const LEDBitMask_t GreenLEDMask = 1<<(GreenLEDOffset-LEDOffset);
00168 const LEDBitMask_t YellowLEDMask = 1<<(YellowLEDOffset-LEDOffset);
00169 const LEDBitMask_t RedLEDMask = 1<<(RedLEDOffset-LEDOffset);
00170 const LEDBitMask_t RFrKneeLEDMask = 1<<(RFrKneeLEDOffset-LEDOffset);
00171 const LEDBitMask_t LFrKneeLEDMask = 1<<(LFrKneeLEDOffset-LEDOffset);
00172 const LEDBitMask_t RMdKneeLEDMask = 1<<(RMdKneeLEDOffset-LEDOffset);
00173 const LEDBitMask_t LMdKneeLEDMask = 1<<(LMdKneeLEDOffset-LEDOffset);
00174 const LEDBitMask_t RBkKneeLEDMask = 1<<(RBkKneeLEDOffset-LEDOffset);
00175 const LEDBitMask_t LBkKneeLEDMask = 1<<(LBkKneeLEDOffset-LEDOffset);
00176 const LEDBitMask_t HeadLEDMask = 1<<(HeadLEDOffset-LEDOffset);
00177
00178
00179 const LEDBitMask_t FaceLEDMask = HeadLEDMask;
00180
00181 const LEDBitMask_t AllLEDMask = (LEDBitMask_t)~0;
00182
00183
00184
00185
00186 const fmat::Column<3> AgentBoundingBoxBaseFrameOffset = fmat::pack(0,0,0);
00187
00188
00189 const fmat::Column<3> AgentBoundingBoxHalfDims = fmat::pack(304.8/2, 304.8/2, 0);
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209 enum ButtonOffset_t { GreenButOffset, RedButOffset, YellowButOffset };
00210
00211
00212 const char* const buttonNames[NumButtons+1] = { "GreenBut", "RedBut", "YellowBut", NULL };
00213
00214
00215
00216 enum SensorOffset_t {
00217 LeftIRDistOffset,
00218 CenterIRDistOffset,
00219 IRDistOffset = CenterIRDistOffset,
00220 RightIRDistOffset,
00221 LeftLuminosityOffset,
00222 CenterLuminosityOffset,
00223 RightLuminosityOffset,
00224 MicVolumeOffset,
00225 MicSpikeCountOffset,
00226 PowerRemainOffset,
00227 PowerThermoOffset,
00228 PowerVoltageOffset,
00229 };
00230
00231
00232 const char* const sensorNames[NumSensors+1] = {
00233 "LeftIRDist", "CenterIRDist", "RightIRDist",
00234 "LeftLuminosity", "CenterLuminosity", "RightLuminosity",
00235 "MicVolume", "MicSpikeCount",
00236 "PowerRemain", "PowerThermo", "PowerVoltage", NULL
00237 };
00238
00239
00240
00241
00242
00243 const char* const outputNames[NumReferenceFrames+1] = {
00244
00245 "RFr:rotor",
00246 "RFr:sweep","RFr:elvtr","RFr:knee",
00247 "LFr:sweep","LFr:elvtr","LFr:knee",
00248 "RMd:sweep","RMd:elvtr","RMd:knee",
00249 "LMd:sweep","LMd:elvtr","LMd:knee",
00250 "RBk:sweep","RBk:elvtr","RBk:knee",
00251 "LBk:sweep","LBk:elvtr","LBk:knee",
00252 "ARM:shldr","ARM:elbow","ARM:wristYaw","ARM:wristPitch","ARM:wristRoll","ARM:gripper",
00253 "NECK:pan","NECK:tilt",
00254
00255
00256
00257 "LED:blue", "LED:green", "LED:yellow", "LED:red",
00258 "LED:RFr:knee","LED:LFr:knee",
00259 "LED:RMd:knee","LED:LMd:knee",
00260 "LED:RBk:knee","LED:LBk:knee",
00261 "LED:NECK:pan",
00262
00263
00264 "BaseFrame",
00265 "RFrFootFrame",
00266 "LFrFootFrame",
00267 "RMdFootFrame",
00268 "LMdFootFrame",
00269 "RBkFootFrame",
00270 "LBkFootFrame",
00271 "GripperFrame",
00272 "CameraFrame",
00273 "LeftIRFrame",
00274 "CenterIRFrame",
00275 "RightIRFrame",
00276 NULL
00277 };
00278
00279
00280 class ChiaraCapabilities : public Capabilities {
00281 public:
00282
00283 ChiaraCapabilities()
00284 : Capabilities(TargetName,NumReferenceFrames,outputNames,NumButtons,buttonNames,NumSensors,sensorNames,PIDJointOffset,NumPIDJoints,LEDOffset,NumLEDs,NumOutputs)
00285 {
00286 frameToIndex["ARM:Wrist"] = WristYawOffset;
00287 frameToIndex["NECK:nod"] = HeadTiltOffset;
00288 frameToIndex["IRFrame"] = IRFrameOffset;
00289 sensorToIndex["IRDist"]=IRDistOffset;
00290 }
00291 };
00292
00293 extern const ChiaraCapabilities capabilities;
00294
00295
00296
00297
00298 const float DefaultPIDs[NumPIDJoints][3] = {
00299 {32,32,0},
00300 {32,32,0}, {32,32,0}, {32,32,0},
00301 {32,32,0}, {32,32,0}, {32,32,0},
00302 {32,32,0}, {32,32,0}, {32,32,0},
00303 {32,32,0}, {32,32,0}, {32,32,0},
00304 {32,32,0}, {32,32,0}, {32,32,0},
00305 {32,32,0}, {32,32,0}, {32,32,0},
00306 {32,32,0}, {32,32,0}, {32,32,0}, {32,32,0}, {32,32,0}, {32,32,0},
00307 {32,32,0}, {32,32,0},
00308 };
00309
00310
00311
00312
00313
00314
00315
00316
00317 const float MaxOutputSpeed[NumOutputs] = {
00318
00319 0.4f,
00320 0.4f,0.4f,0.4f,
00321 0.4f,0.4f,0.4f,
00322 0.4f,0.4f,0.4f,
00323 0.4f,0.4f,0.4f,
00324 0.4f,0.4f,0.4f,
00325 0.4f,0.4f,0.4f,
00326 0.4f,0.4f,0.4f,0.4f,0.4f,0.4f,
00327 0.4f,0.4f,
00328
00329 0,0,
00330 0,0,
00331 0,0,
00332 0,
00333 0,0,0,0,
00334 };
00335
00336 #ifndef RAD
00337
00338 #define RAD(deg) (((deg) * (float)M_PI ) / 180.0f)
00339
00340 #define __RI_RAD_FLAG
00341 #endif
00342
00343
00344 const float outputRanges[NumOutputs][2] = {
00345
00346 {RAD(-17),RAD(90)},
00347 {RAD(-102),RAD(67)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00348 {RAD(-53),RAD(58)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00349 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00350 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00351 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00352 {RAD(-73),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00353 {RAD(-95),RAD(95)}, {RAD(-95),RAD(95)}, {RAD(-100),RAD(100)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)},
00354 {RAD(-150),RAD(150)}, {RAD(-92),RAD(75)},
00355
00356
00357 {0,1}, {0,1},
00358 {0,1}, {0,1},
00359 {0,1}, {0,1},
00360 {0,1},
00361 {0,1}, {0,1}, {0,1}, {0,1},
00362 };
00363
00364
00365
00366 const float mechanicalLimits[NumOutputs][2] = {
00367
00368 {RAD(-17),RAD(90)},
00369 {RAD(-102),RAD(67)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00370 {RAD(-53),RAD(58)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00371 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00372 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00373 {RAD(-53),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00374 {RAD(-73),RAD(53)}, {RAD(-100),RAD(100)}, {RAD(-55),RAD(160)},
00375 {RAD(-95),RAD(95)}, {RAD(-95),RAD(95)}, {RAD(-100),RAD(100)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)}, {RAD(-90),RAD(90)},
00376 {RAD(-150),RAD(150)}, {RAD(-92),RAD(75)},
00377
00378
00379 {0,1}, {0,1},
00380 {0,1}, {0,1},
00381 {0,1}, {0,1},
00382 {0,1},
00383 {0,1}, {0,1}, {0,1}, {0,1},
00384 };
00385
00386 #ifdef __RI_RAD_FLAG
00387 #undef RAD
00388 #undef __RI_RAD_FLAG
00389 #endif
00390 }
00391
00392
00393
00394
00395
00396
00397 #endif