00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <iostream>
00030 using namespace std;
00031
00032 #include "../../sbpl/headers.h"
00033
00034 #include <queue>
00035
00036
00037 #define COSTMULT 1000
00038
00039
00040
00041
00042
00043
00044 #define XYTO2DIND(x,y) ((x) + (y)*EnvROBARMCfg.EnvWidth_c)
00045
00046 #define DIRECTIONS 8
00047 int dx[DIRECTIONS] = {1, 1, 1, 0, 0, -1, -1, -1};
00048 int dy[DIRECTIONS] = {1, 0, -1, 1, -1, -1, 0, 1};
00049
00050
00051
00052
00053
00054
00055
00056 static unsigned int inthash(unsigned int key)
00057 {
00058 key += (key << 12);
00059 key ^= (key >> 22);
00060 key += (key << 4);
00061 key ^= (key >> 9);
00062 key += (key << 10);
00063 key ^= (key >> 2);
00064 key += (key << 7);
00065 key ^= (key >> 12);
00066 return key;
00067 }
00068
00069
00070
00071
00072 unsigned int EnvironmentROBARM::GETHASHBIN(short unsigned int* coord, int numofcoord)
00073 {
00074
00075 int val = 0;
00076
00077 for(int i = 0; i < numofcoord; i++)
00078 {
00079 val += inthash(coord[i]) << i;
00080 }
00081
00082 return inthash(val) & (EnvROBARM.HashTableSize-1);
00083 }
00084
00085
00086
00087
00088 void EnvironmentROBARM::PrintHashTableHist()
00089 {
00090 int s0=0, s1=0, s50=0, s100=0, s200=0, s300=0, slarge=0;
00091
00092 for(int j = 0; j < EnvROBARM.HashTableSize; j++)
00093 {
00094 if((int)EnvROBARM.Coord2StateIDHashTable[j].size() == 0)
00095 s0++;
00096 else if((int)EnvROBARM.Coord2StateIDHashTable[j].size() < 50)
00097 s1++;
00098 else if((int)EnvROBARM.Coord2StateIDHashTable[j].size() < 100)
00099 s50++;
00100 else if((int)EnvROBARM.Coord2StateIDHashTable[j].size() < 200)
00101 s100++;
00102 else if((int)EnvROBARM.Coord2StateIDHashTable[j].size() < 300)
00103 s200++;
00104 else if((int)EnvROBARM.Coord2StateIDHashTable[j].size() < 400)
00105 s300++;
00106 else
00107 slarge++;
00108 }
00109 SBPL_PRINTF("hash table histogram: 0:%d, <50:%d, <100:%d, <200:%d, <300:%d, <400:%d >400:%d\n",
00110 s0,s1, s50, s100, s200,s300,slarge);
00111 }
00112
00113
00114 EnvROBARMHashEntry_t* EnvironmentROBARM::GetHashEntry(short unsigned int* coord, int numofcoord, bool bIsGoal)
00115 {
00116
00117
00118
00119 if(bIsGoal)
00120 {
00121 return EnvROBARM.goalHashEntry;
00122 }
00123
00124 int binid = GETHASHBIN(coord, numofcoord);
00125
00126 #if DEBUG
00127 if ((int)EnvROBARM.Coord2StateIDHashTable[binid].size() > 500)
00128 {
00129 SBPL_PRINTF("WARNING: Hash table has a bin %d (coord0=%d) of size %d\n",
00130 binid, coord[0], EnvROBARM.Coord2StateIDHashTable[binid].size());
00131
00132 PrintHashTableHist();
00133 }
00134 #endif
00135
00136
00137 for(int ind = 0; ind < (int)EnvROBARM.Coord2StateIDHashTable[binid].size(); ind++)
00138 {
00139 int j = 0;
00140 for(j = 0; j < numofcoord; j++)
00141 {
00142 if( EnvROBARM.Coord2StateIDHashTable[binid][ind]->coord[j] != coord[j])
00143 {
00144 break;
00145 }
00146 }
00147 if (j == numofcoord)
00148 {
00149
00150 return EnvROBARM.Coord2StateIDHashTable[binid][ind];
00151 }
00152 }
00153
00154
00155
00156 return NULL;
00157 }
00158
00159
00160 EnvROBARMHashEntry_t* EnvironmentROBARM::CreateNewHashEntry(short unsigned int* coord, int numofcoord, short unsigned int endeffx, short unsigned int endeffy)
00161 {
00162 int i;
00163
00164
00165
00166 EnvROBARMHashEntry_t* HashEntry = new EnvROBARMHashEntry_t;
00167
00168 memcpy(HashEntry->coord, coord, numofcoord*sizeof(short unsigned int));
00169 HashEntry->endeffx = endeffx;
00170 HashEntry->endeffy = endeffy;
00171
00172
00173 HashEntry->stateID = EnvROBARM.StateID2CoordTable.size();
00174
00175
00176 EnvROBARM.StateID2CoordTable.push_back(HashEntry);
00177
00178
00179 i = GETHASHBIN(HashEntry->coord, numofcoord);
00180
00181
00182 EnvROBARM.Coord2StateIDHashTable[i].push_back(HashEntry);
00183
00184
00185 int* entry = new int [NUMOFINDICES_STATEID2IND];
00186 StateID2IndexMapping.push_back(entry);
00187 for(i = 0; i < NUMOFINDICES_STATEID2IND; i++)
00188 {
00189 StateID2IndexMapping[HashEntry->stateID][i] = -1;
00190 }
00191
00192 if(HashEntry->stateID != (int)StateID2IndexMapping.size()-1)
00193 {
00194 SBPL_ERROR("ERROR in Env... function: last state has incorrect stateID\n");
00195 throw new SBPL_Exception();
00196 }
00197
00198
00199
00200
00201 return HashEntry;
00202 }
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212 int EnvironmentROBARM::distanceincoord(unsigned short* statecoord1, unsigned short* statecoord2)
00213 {
00214 int totaldiff = 0;
00215
00216 for(int i = 0; i < NUMOFLINKS; i++)
00217 {
00218 int diff = abs(statecoord1[i] - statecoord2[i]);
00219
00220 totaldiff = __max(totaldiff, __min(diff, EnvROBARMCfg.anglevals[i] - diff));
00221 }
00222
00223
00224 return totaldiff;
00225 }
00226
00227
00228 void EnvironmentROBARM::ReInitializeState2D(State2D* state)
00229 {
00230 state->g = INFINITECOST;
00231 state->iterationclosed = 0;
00232 }
00233
00234 void EnvironmentROBARM::InitializeState2D(State2D* state, short unsigned int x, short unsigned int y)
00235 {
00236 state->g = INFINITECOST;
00237 state->iterationclosed = 0;
00238 state->x = x;
00239 state->y = y;
00240 }
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309 void EnvironmentROBARM::Search2DwithQueue(State2D** statespace, int* HeurGrid, int searchstartx, int searchstarty)
00310 {
00311 State2D* ExpState;
00312 int newx, newy,x,y;
00313
00314
00315 queue<State2D*> Queue;
00316
00317
00318 for (x = 0; x < EnvROBARMCfg.EnvWidth_c; x++)
00319 {
00320 for (y = 0; y < EnvROBARMCfg.EnvHeight_c; y++)
00321 {
00322 HeurGrid[XYTO2DIND(x,y)] = INFINITECOST;
00323 ReInitializeState2D(&statespace[x][y]);
00324 }
00325 }
00326
00327
00328 statespace[searchstartx][searchstarty].g = 0;
00329 Queue.push(&statespace[searchstartx][searchstarty]);
00330
00331
00332 while((int)Queue.size() > 0)
00333 {
00334
00335
00336 ExpState = Queue.front();
00337 Queue.pop();
00338
00339
00340 if(ExpState->iterationclosed == 1)
00341 continue;
00342
00343
00344 ExpState->iterationclosed = 1;
00345
00346
00347 HeurGrid[XYTO2DIND(ExpState->x, ExpState->y)] = ExpState->g;
00348
00349
00350 for(int d = 0; d < DIRECTIONS; d++)
00351 {
00352 newx = ExpState->x + dx[d];
00353 newy = ExpState->y + dy[d];
00354
00355
00356 if(0 > newx || newx >= EnvROBARMCfg.EnvWidth_c ||
00357 0 > newy || newy >= EnvROBARMCfg.EnvHeight_c ||
00358 EnvROBARMCfg.Grid2D[newx][newy] == 1)
00359 continue;
00360
00361
00362 if(statespace[newx][newy].g != INFINITECOST &&
00363 statespace[newx][newy].g > ExpState->g + 1)
00364 {
00365 SBPL_ERROR("ERROR: incorrect heuristic computation\n");
00366 throw new SBPL_Exception();
00367 }
00368
00369
00370 if(statespace[newx][newy].iterationclosed == 0 &&
00371 statespace[newx][newy].g == INFINITECOST)
00372 {
00373
00374 Queue.push(&statespace[newx][newy]);
00375
00376
00377 statespace[newx][newy].g = ExpState->g + 1;
00378 }
00379 }
00380 }
00381 }
00382
00383
00384 void EnvironmentROBARM::Create2DStateSpace(State2D*** statespace2D)
00385 {
00386 int x,y;
00387
00388
00389 *statespace2D = new State2D* [EnvROBARMCfg.EnvWidth_c];
00390 for (x = 0; x < EnvROBARMCfg.EnvWidth_c; x++)
00391 {
00392 (*statespace2D)[x] = new State2D [EnvROBARMCfg.EnvHeight_c];
00393 for(y = 0; y < EnvROBARMCfg.EnvWidth_c; y++)
00394 {
00395 InitializeState2D(&(*statespace2D)[x][y], x,y);
00396 }
00397 }
00398 }
00399
00400 void EnvironmentROBARM::Delete2DStateSpace(State2D*** statespace2D)
00401 {
00402 int x;
00403
00404
00405 for (x = 0; x < EnvROBARMCfg.EnvWidth_c; x++)
00406 {
00407 delete [] (*statespace2D)[x];
00408 }
00409 delete *statespace2D;
00410
00411 }
00412
00413 void EnvironmentROBARM::ComputeHeuristicValues()
00414 {
00415 int i;
00416
00417 SBPL_PRINTF("Running 2D BFS to compute heuristics\n");
00418
00419
00420 int hsize = XYTO2DIND(EnvROBARMCfg.EnvWidth_c-1, EnvROBARMCfg.EnvHeight_c-1)+1;
00421 EnvROBARM.Heur = new int* [hsize];
00422 for(i = 0; i < hsize; i++)
00423 {
00424 EnvROBARM.Heur[i] = new int [hsize];
00425 }
00426
00427
00428
00429 State2D** statespace2D;
00430 Create2DStateSpace(&statespace2D);
00431
00432 for(int x = 0; x < EnvROBARMCfg.EnvWidth_c; x++)
00433 {
00434 for(int y = 0; y < EnvROBARMCfg.EnvHeight_c; y++)
00435 {
00436 int hind = XYTO2DIND(x,y);
00437
00438
00439 Search2DwithQueue(statespace2D, &EnvROBARM.Heur[hind][0], x, y);
00440 }
00441
00442 }
00443 Delete2DStateSpace(&statespace2D);
00444
00445
00446 SBPL_PRINTF("done\n");
00447 }
00448
00449
00450
00451
00452
00453
00454
00455
00456
00457
00458
00459
00460
00461
00462
00463
00464
00465
00466
00467
00468 void EnvironmentROBARM::printangles(FILE* fOut, short unsigned int* coord, bool bGoal, bool bVerbose, bool bLocal)
00469 {
00470 double angles[NUMOFLINKS];
00471 int i;
00472 short unsigned int x,y;
00473
00474 ComputeContAngles(coord, angles);
00475 if(bVerbose)
00476 SBPL_FPRINTF(fOut, "angles: ");
00477 for(i = 0; i < NUMOFLINKS; i++)
00478 {
00479 if(!bLocal)
00480 SBPL_FPRINTF(fOut, "%f ", angles[i]);
00481 else
00482 {
00483 if(i > 0)
00484 SBPL_FPRINTF(fOut, "%f ", angles[i]-angles[i-1]);
00485 else
00486 SBPL_FPRINTF(fOut, "%f ", angles[i]);
00487 }
00488 }
00489 ComputeEndEffectorPos(angles, &x, &y);
00490 if(bGoal)
00491 {
00492 x = EnvROBARMCfg.EndEffGoalX_c;
00493 y = EnvROBARMCfg.EndEffGoalY_c;
00494 }
00495 if(bVerbose)
00496 SBPL_FPRINTF(fOut, "endeff: %d %d", x,y);
00497 else
00498 SBPL_FPRINTF(fOut, "%d %d", x,y);
00499
00500 SBPL_FPRINTF(fOut, "\n");
00501
00502 }
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535
00536
00537
00538
00539
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578
00579
00580
00581
00582
00583
00584
00585
00586
00587
00588
00589
00590
00591
00592
00593
00594
00595
00596
00597
00598
00599
00600
00601
00602
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644
00645
00646
00647
00648
00649
00650
00651
00652
00653
00654
00655 void EnvironmentROBARM::ReadConfiguration(FILE* fCfg)
00656 {
00657 char sTemp[1024];
00658 int dTemp;
00659 int x, y, i;
00660
00661
00662 if(fscanf(fCfg, "%s", sTemp) != 1){
00663 SBPL_ERROR("ERROR: ran out of env file early\n");
00664 throw new SBPL_Exception();
00665 }
00666 if(fscanf(fCfg, "%s", sTemp) != 1){
00667 SBPL_ERROR("ERROR: ran out of env file early\n");
00668 throw new SBPL_Exception();
00669 }
00670 EnvROBARMCfg.EnvWidth_m = atof(sTemp);
00671 if(fscanf(fCfg, "%s", sTemp) != 1){
00672 SBPL_ERROR("ERROR: ran out of env file early\n");
00673 throw new SBPL_Exception();
00674 }
00675 EnvROBARMCfg.EnvHeight_m = atof(sTemp);
00676
00677
00678
00679 if(fscanf(fCfg, "%s", sTemp) != 1){
00680 SBPL_ERROR("ERROR: ran out of env file early\n");
00681 throw new SBPL_Exception();
00682 }
00683 if(fscanf(fCfg, "%s", sTemp) != 1){
00684 SBPL_ERROR("ERROR: ran out of env file early\n");
00685 throw new SBPL_Exception();
00686 }
00687 EnvROBARMCfg.EnvWidth_c = atoi(sTemp);
00688 if(fscanf(fCfg, "%s", sTemp) != 1){
00689 SBPL_ERROR("ERROR: ran out of env file early\n");
00690 throw new SBPL_Exception();
00691 }
00692 EnvROBARMCfg.EnvHeight_c = atoi(sTemp);
00693
00694
00695 if(fscanf(fCfg, "%s", sTemp) != 1){
00696 SBPL_ERROR("ERROR: ran out of env file early\n");
00697 throw new SBPL_Exception();
00698 }
00699 if(fscanf(fCfg, "%s", sTemp) != 1){
00700 SBPL_ERROR("ERROR: ran out of env file early\n");
00701 throw new SBPL_Exception();
00702 }
00703 EnvROBARMCfg.BaseX_c = atoi(sTemp);
00704
00705
00706 if(fscanf(fCfg, "%s", sTemp) != 1){
00707 SBPL_ERROR("ERROR: ran out of env file early\n");
00708 throw new SBPL_Exception();
00709 }
00710 for(i = 0; i < NUMOFLINKS; i++)
00711 {
00712 if(fscanf(fCfg, "%s", sTemp) != 1){
00713 SBPL_ERROR("ERROR: ran out of env file early\n");
00714 throw new SBPL_Exception();
00715 }
00716 EnvROBARMCfg.LinkLength_m[i] = atof(sTemp);
00717 }
00718
00719
00720 if(fscanf(fCfg, "%s", sTemp) != 1){
00721 SBPL_ERROR("ERROR: ran out of env file early\n");
00722 throw new SBPL_Exception();
00723 }
00724 for(i = 0; i < NUMOFLINKS; i++)
00725 {
00726 if(fscanf(fCfg, "%s", sTemp) != 1){
00727 SBPL_ERROR("ERROR: ran out of env file early\n");
00728 throw new SBPL_Exception();
00729 }
00730 EnvROBARMCfg.LinkStartAngles_d[i] = atoi(sTemp);
00731 }
00732
00733
00734 if(fscanf(fCfg, "%s", sTemp) != 1){
00735 SBPL_ERROR("ERROR: ran out of env file early\n");
00736 throw new SBPL_Exception();
00737 }
00738 if(strcmp(sTemp, "endeffectorgoal(cells):") == 0)
00739 {
00740
00741 if(fscanf(fCfg, "%s", sTemp) != 1){
00742 SBPL_ERROR("ERROR: ran out of env file early\n");
00743 throw new SBPL_Exception();
00744 }
00745 EnvROBARMCfg.EndEffGoalX_c = atoi(sTemp);
00746 if(fscanf(fCfg, "%s", sTemp) != 1){
00747 SBPL_ERROR("ERROR: ran out of env file early\n");
00748 throw new SBPL_Exception();
00749 }
00750 EnvROBARMCfg.EndEffGoalY_c = atoi(sTemp);
00751
00752
00753 EnvROBARMCfg.LinkGoalAngles_d[0] = INVALID_NUMBER;
00754 }
00755 else if(strcmp(sTemp, "linkgoalangles(degrees):") == 0)
00756 {
00757 double goalangles[NUMOFLINKS];
00758
00759
00760 for(i = 0; i < NUMOFLINKS; i++)
00761 {
00762 if(fscanf(fCfg, "%s", sTemp) != 1){
00763 SBPL_ERROR("ERROR: ran out of env file early\n");
00764 throw new SBPL_Exception();
00765 }
00766 EnvROBARMCfg.LinkGoalAngles_d[i] = atoi(sTemp);
00767 }
00768
00769 for(i = 0; i < NUMOFLINKS; i++)
00770 {
00771 goalangles[i] = PI_CONST*(EnvROBARMCfg.LinkGoalAngles_d[i]/180.0);
00772 }
00773 ComputeEndEffectorPos(goalangles, &EnvROBARMCfg.EndEffGoalX_c, &EnvROBARMCfg.EndEffGoalY_c);
00774 }
00775 else
00776 {
00777 SBPL_ERROR("ERROR: invalid string encountered=%s\n", sTemp);
00778 throw new SBPL_Exception();
00779 }
00780
00781
00782
00783 EnvROBARMCfg.Grid2D = new char* [EnvROBARMCfg.EnvWidth_c];
00784 for (x = 0; x < EnvROBARMCfg.EnvWidth_c; x++)
00785 {
00786 EnvROBARMCfg.Grid2D[x] = new char [EnvROBARMCfg.EnvHeight_c];
00787 }
00788
00789
00790
00791 if(fscanf(fCfg, "%s", sTemp) != 1){
00792 SBPL_ERROR("ERROR: ran out of env file early\n");
00793 throw new SBPL_Exception();
00794 }
00795 for (y = 0; y < EnvROBARMCfg.EnvHeight_c; y++)
00796 for (x = 0; x < EnvROBARMCfg.EnvWidth_c; x++)
00797 {
00798 if(fscanf(fCfg, "%d", &dTemp) != 1)
00799 {
00800 SBPL_ERROR("ERROR: incorrect format of config file\n");
00801 throw new SBPL_Exception();
00802 }
00803 EnvROBARMCfg.Grid2D[x][y] = dTemp;
00804 }
00805
00806
00807
00808 EnvROBARMCfg.GridCellWidth = EnvROBARMCfg.EnvWidth_m/EnvROBARMCfg.EnvWidth_c;
00809 if(EnvROBARMCfg.GridCellWidth != EnvROBARMCfg.EnvHeight_m/EnvROBARMCfg.EnvHeight_c)
00810 {
00811 SBPL_ERROR("ERROR: The cell should be square\n");
00812 throw new SBPL_Exception();
00813 }
00814 }
00815
00816
00817
00818 void EnvironmentROBARM::DiscretizeAngles()
00819 {
00820 int i;
00821 double HalfGridCell = EnvROBARMCfg.GridCellWidth/2.0;
00822
00823 for(i = 0; i < NUMOFLINKS; i++)
00824 {
00825 EnvROBARMCfg.angledelta[i] = 2*asin(HalfGridCell/EnvROBARMCfg.LinkLength_m[i]);
00826 EnvROBARMCfg.anglevals[i] = (int)(2.0*PI_CONST/EnvROBARMCfg.angledelta[i]+0.99999999);
00827 }
00828
00829 }
00830
00831
00832
00833 void EnvironmentROBARM::ComputeContAngles(short unsigned int coord[NUMOFLINKS], double angle[NUMOFLINKS])
00834 {
00835 int i;
00836
00837 for(i = 0; i < NUMOFLINKS; i++)
00838 {
00839 angle[i] = coord[i]*EnvROBARMCfg.angledelta[i];
00840 }
00841 }
00842
00843 void EnvironmentROBARM::ComputeCoord(double angle[NUMOFLINKS], short unsigned int coord[NUMOFLINKS])
00844 {
00845 int i;
00846
00847 for(i = 0; i < NUMOFLINKS; i++)
00848 {
00849 coord[i] = (int)((angle[i] + EnvROBARMCfg.angledelta[i]*0.5)/EnvROBARMCfg.angledelta[i]);
00850 if(coord[i] == EnvROBARMCfg.anglevals[i])
00851 coord[i] = 0;
00852 }
00853 }
00854
00855
00856
00857 unsigned int EnvironmentROBARM::GetHeurBasedonCoord(short unsigned int coord[NUMOFLINKS])
00858 {
00859 short unsigned int endeffx, endeffy;
00860 double angles[NUMOFLINKS];
00861 unsigned int h;
00862
00863 ComputeContAngles(coord, angles);
00864 ComputeEndEffectorPos(angles, &endeffx, &endeffy);
00865
00866 #if INFORMED
00867 h = HeurGrid[endeffx][endeffy];
00868 #else
00869 h = 0;
00870 #endif
00871
00872 return h;
00873
00874 }
00875
00876 void EnvironmentROBARM::Cell2ContXY(int x, int y, double *pX, double *pY)
00877 {
00878 *pX = x*EnvROBARMCfg.GridCellWidth + EnvROBARMCfg.GridCellWidth*0.5;
00879 *pY = y*EnvROBARMCfg.GridCellWidth + EnvROBARMCfg.GridCellWidth*0.5;
00880 }
00881
00882 void EnvironmentROBARM::ContXY2Cell(double x, double y, short unsigned int* pX, short unsigned int *pY)
00883 {
00884
00885 *pX = (int)(x/EnvROBARMCfg.GridCellWidth);
00886 if( x < 0) *pX = 0;
00887 if( *pX >= EnvROBARMCfg.EnvWidth_c) *pX = EnvROBARMCfg.EnvWidth_c-1;
00888
00889 *pY = (int)(y/EnvROBARMCfg.GridCellWidth);
00890 if( y < 0) *pY = 0;
00891 if( *pY >= EnvROBARMCfg.EnvHeight_c) *pY = EnvROBARMCfg.EnvHeight_c-1;
00892 }
00893
00894
00895 int EnvironmentROBARM::ComputeEndEffectorPos(double angles[NUMOFLINKS], short unsigned int* pX, short unsigned int* pY)
00896 {
00897 double x,y;
00898 int i;
00899 int retval = 1;
00900
00901
00902 Cell2ContXY(EnvROBARMCfg.BaseX_c, EnvROBARMCfg.EnvHeight_c-1, &x, &y);
00903
00904
00905 for(i = 0; i < NUMOFLINKS; i++)
00906 {
00907 x = x + EnvROBARMCfg.LinkLength_m[i]*cos(angles[i]);
00908 y = y - EnvROBARMCfg.LinkLength_m[i]*sin(angles[i]);
00909 }
00910
00911 if(x < 0 || x >= EnvROBARMCfg.EnvWidth_m || y < 0 || y >= EnvROBARMCfg.EnvHeight_m)
00912 retval = 0;
00913
00914
00915 ContXY2Cell(x, y, pX, pY);
00916
00917
00918 return retval;
00919 }
00920
00921
00922
00923
00924 int EnvironmentROBARM::IsValidLineSegment(double x0, double y0, double x1, double y1, char **Grid2D,
00925 vector<CELLV>* pTestedCells)
00926
00927 {
00928 bresenham_param_t params;
00929 int nX, nY;
00930 short unsigned int nX0, nY0, nX1, nY1;
00931 int retvalue = 1;
00932 CELLV tempcell;
00933
00934
00935 if(x0 < 0 || x0 >= EnvROBARMCfg.EnvWidth_m ||
00936 x1 < 0 || x1 >= EnvROBARMCfg.EnvWidth_m ||
00937 y0 < 0 || y0 >= EnvROBARMCfg.EnvHeight_m ||
00938 y1 < 0 || y1 >= EnvROBARMCfg.EnvHeight_m)
00939 return 0;
00940
00941 ContXY2Cell(x0, y0, &nX0, &nY0);
00942 ContXY2Cell(x1, y1, &nX1, &nY1);
00943
00944
00945
00946 get_bresenham_parameters(nX0, nY0, nX1, nY1, ¶ms);
00947 do {
00948 get_current_point(¶ms, &nX, &nY);
00949 if(Grid2D[nX][nY] == 1)
00950 {
00951 if(pTestedCells == NULL)
00952 return 0;
00953 else
00954 retvalue = 0;
00955 }
00956
00957
00958 if(pTestedCells)
00959 {
00960 tempcell.bIsObstacle = (Grid2D[nX][nY] == 1);
00961 tempcell.x = nX;
00962 tempcell.y = nY;
00963 pTestedCells->push_back(tempcell);
00964 }
00965 } while (get_next_point(¶ms));
00966
00967
00968 return retvalue;
00969 }
00970
00971 int EnvironmentROBARM::IsValidCoord(short unsigned int coord[NUMOFLINKS], char** Grid2D ,
00972 vector<CELLV>* pTestedCells )
00973 {
00974 double angles[NUMOFLINKS];
00975 int retvalue = 1;
00976
00977 if(Grid2D == NULL)
00978 Grid2D = EnvROBARMCfg.Grid2D;
00979
00980 #if ENDEFF_CHECK_ONLY
00981 int endeffx, endeffy;
00982
00983
00984 ComputeContAngles(coord, angles);
00985 if(ComputeEndEffectorPos(angles, &endeffx, &endeffy) == false)
00986 return 0;
00987
00988
00989 if(endeffx < 0 || endeffx >= EnvROBARMCfg.EnvWidth_c ||
00990 endeffy < 0 || endeffy >= EnvROBARMCfg.EnvHeight_c)
00991 return 0;
00992
00993 if(pTestedCells)
00994 {
00995 CELLV tempcell;
00996 tempcell.IsObstacle = Grid2D[endeffx][endeffy];
00997 tempcell.x = endeffx;
00998 tempcell.y = endeffy;
00999
01000 pTestedCells->push_back(tempcell);
01001 }
01002
01003
01004 if(Grid2D[endeffx][endeffy] == 1)
01005 return 0;
01006 #else
01007 double x0,y0,x1,y1;
01008 int i;
01009
01010
01011 ComputeContAngles(coord, angles);
01012
01013
01014 Cell2ContXY(EnvROBARMCfg.BaseX_c, EnvROBARMCfg.EnvHeight_c-1,&x1,&y1);
01015 for(i = 0; i < NUMOFLINKS; i++)
01016 {
01017
01018 x0 = x1;
01019 y0 = y1;
01020 x1 = x0 + EnvROBARMCfg.LinkLength_m[i]*cos(angles[i]);
01021 y1 = y0 - EnvROBARMCfg.LinkLength_m[i]*sin(angles[i]);
01022
01023
01024 if(!IsValidLineSegment(x0,y0,x1,y1,Grid2D, pTestedCells))
01025 {
01026 if(pTestedCells == NULL)
01027 return 0;
01028 else
01029 retvalue = 0;
01030 }
01031
01032 }
01033
01034 #endif
01035
01036 return retvalue;
01037
01038 }
01039
01040 int EnvironmentROBARM::cost(short unsigned int state1coord[], short unsigned int state2coord[])
01041 {
01042
01043 if(!IsValidCoord(state1coord ) || !IsValidCoord(state2coord))
01044 return INFINITECOST;
01045
01046 #if UNIFORM_COST
01047 return 1;
01048 #else
01049 int i;
01050
01051 for(i = 0; i < NUMOFLINKS; i++)
01052 {
01053 if(state1coord[i] != state2coord[i])
01054 return (NUMOFLINKS-i);
01055
01056 }
01057
01058 SBPL_ERROR("ERROR: cost on the same states is called:\n");
01059
01060
01061
01062 throw new SBPL_Exception();
01063
01064 #endif
01065
01066 }
01067
01068 void EnvironmentROBARM::InitializeEnvConfig()
01069 {
01070
01071 DiscretizeAngles();
01072 }
01073
01074 bool EnvironmentROBARM::InitializeEnvironment()
01075 {
01076 short unsigned int coord[NUMOFLINKS];
01077 double startangles[NUMOFLINKS];
01078 double angles[NUMOFLINKS];
01079 int i;
01080 short unsigned int endeffx, endeffy;
01081
01082
01083 EnvROBARM.HashTableSize = 32*1024;
01084 EnvROBARM.Coord2StateIDHashTable = new vector<EnvROBARMHashEntry_t*>[EnvROBARM.HashTableSize];
01085
01086
01087 EnvROBARM.StateID2CoordTable.clear();
01088
01089
01090 for(i = 0; i < NUMOFLINKS; i++)
01091 {
01092 startangles[i] = PI_CONST*(EnvROBARMCfg.LinkStartAngles_d[i]/180.0);
01093 }
01094
01095 ComputeCoord(startangles, coord);
01096 ComputeContAngles(coord, angles);
01097 ComputeEndEffectorPos(angles, &endeffx, &endeffy);
01098
01099
01100 EnvROBARM.startHashEntry = CreateNewHashEntry(coord, NUMOFLINKS, endeffx, endeffy);
01101
01102
01103
01104
01105 for(i = 0; i < NUMOFLINKS; i++)
01106 {
01107 coord[i] = 0;
01108 }
01109 EnvROBARM.goalHashEntry = CreateNewHashEntry(coord, NUMOFLINKS, EnvROBARMCfg.EndEffGoalX_c, EnvROBARMCfg.EndEffGoalY_c);
01110
01111
01112
01113 if(!IsValidCoord(EnvROBARM.startHashEntry->coord) || EnvROBARMCfg.EndEffGoalX_c >= EnvROBARMCfg.EnvWidth_c ||
01114 EnvROBARMCfg.EndEffGoalY_c >= EnvROBARMCfg.EnvHeight_c)
01115 {
01116 SBPL_PRINTF("Either start or goal configuration is invalid\n");
01117 return false;
01118 }
01119
01120
01121 EnvROBARM.Heur = NULL;
01122
01123 return true;
01124 }
01125
01126
01127
01128
01129
01130
01131
01132 bool EnvironmentROBARM::InitializeEnv(const char* sEnvFile)
01133 {
01134
01135 FILE* fCfg = fopen(sEnvFile, "r");
01136 if(fCfg == NULL)
01137 {
01138 SBPL_ERROR("ERROR: unable to open %s\n", sEnvFile);
01139 throw new SBPL_Exception();
01140 }
01141 ReadConfiguration(fCfg);
01142 fclose(fCfg);
01143
01144
01145 InitializeEnvConfig();
01146
01147
01148 if(InitializeEnvironment() == false)
01149 return false;
01150
01151
01152 ComputeHeuristicValues();
01153
01154 return true;
01155 }
01156
01157
01158
01159 bool EnvironmentROBARM::InitializeMDPCfg(MDPConfig *MDPCfg)
01160 {
01161
01162 MDPCfg->goalstateid = EnvROBARM.goalHashEntry->stateID;
01163 MDPCfg->startstateid = EnvROBARM.startHashEntry->stateID;
01164
01165 return true;
01166 }
01167
01168
01169 int EnvironmentROBARM::GetFromToHeuristic(int FromStateID, int ToStateID)
01170 {
01171 #if USE_HEUR==0
01172 return 0;
01173 #endif
01174
01175
01176 #if DEBUG
01177 if(FromStateID >= (int)EnvROBARM.StateID2CoordTable.size()
01178 || ToStateID >= (int)EnvROBARM.StateID2CoordTable.size())
01179 {
01180 SBPL_ERROR("ERROR in EnvROBARM... function: stateID illegal\n");
01181 throw new SBPL_Exception();
01182 }
01183 #endif
01184
01185
01186 EnvROBARMHashEntry_t* FromHashEntry = EnvROBARM.StateID2CoordTable[FromStateID];
01187 EnvROBARMHashEntry_t* ToHashEntry = EnvROBARM.StateID2CoordTable[ToStateID];
01188
01189
01190 int h = EnvROBARM.Heur[XYTO2DIND(ToHashEntry->endeffx, ToHashEntry->endeffy)][XYTO2DIND(FromHashEntry->endeffx, FromHashEntry->endeffy)];
01191
01192
01193
01194
01195
01196
01197
01198
01199
01200
01201
01202
01203
01204
01205
01206
01207 return h;
01208
01209 }
01210
01211
01212 int EnvironmentROBARM::GetGoalHeuristic(int stateID)
01213 {
01214 #if USE_HEUR==0
01215 return 0;
01216 #endif
01217
01218 #if DEBUG
01219 if(stateID >= (int)EnvROBARM.StateID2CoordTable.size())
01220 {
01221 SBPL_ERROR("ERROR in EnvROBARM... function: stateID illegal\n");
01222 throw new SBPL_Exception();
01223 }
01224 #endif
01225
01226
01227 return GetFromToHeuristic(stateID, EnvROBARM.goalHashEntry->stateID);
01228 }
01229
01230 int EnvironmentROBARM::GetStartHeuristic(int stateID)
01231 {
01232 #if USE_HEUR==0
01233 return 0;
01234 #endif
01235
01236
01237 #if DEBUG
01238 if(stateID >= (int)EnvROBARM.StateID2CoordTable.size())
01239 {
01240 SBPL_ERROR("ERROR in EnvROBARM... function: stateID illegal\n");
01241 throw new SBPL_Exception();
01242 }
01243 #endif
01244
01245
01246
01247 SBPL_ERROR("ERROR in EnvROBARM.. function: GetStartHeuristic is undefined\n");
01248 throw new SBPL_Exception();
01249
01250 return 0;
01251
01252
01253 }
01254
01255
01256 void EnvironmentROBARM::SetAllPreds(CMDPSTATE* state)
01257 {
01258
01259
01260 SBPL_ERROR("ERROR in EnvROBARM... function: SetAllPreds is undefined\n");
01261 throw new SBPL_Exception();
01262 }
01263
01264
01265 int EnvironmentROBARM::SizeofCreatedEnv()
01266 {
01267 return EnvROBARM.StateID2CoordTable.size();
01268
01269 }
01270
01271 void EnvironmentROBARM::PrintState(int stateID, bool bVerbose, FILE* fOut )
01272 {
01273 bool bLocal = false;
01274
01275 #if DEBUG
01276 if(stateID >= (int)EnvROBARM.StateID2CoordTable.size())
01277 {
01278 SBPL_ERROR("ERROR in EnvROBARM... function: stateID illegal (2)\n");
01279 throw new SBPL_Exception();
01280 }
01281 #endif
01282
01283 if(fOut == NULL)
01284 fOut = stdout;
01285
01286 EnvROBARMHashEntry_t* HashEntry = EnvROBARM.StateID2CoordTable[stateID];
01287
01288 bool bGoal = false;
01289 if(stateID == EnvROBARM.goalHashEntry->stateID)
01290 bGoal = true;
01291
01292 if(stateID == EnvROBARM.goalHashEntry->stateID && bVerbose)
01293 {
01294 SBPL_FPRINTF(fOut, "the state is a goal state\n");
01295 bGoal = true;
01296 }
01297
01298 if(bLocal)
01299 {
01300 printangles(fOut, HashEntry->coord, bGoal, bVerbose, true);
01301 }
01302 else
01303 printangles(fOut, HashEntry->coord, bGoal, bVerbose, false);
01304 }
01305
01306
01307
01308
01309 void EnvironmentROBARM::PrintSuccGoal(int SourceStateID, int costtogoal, bool bVerbose, bool bLocal , FILE* fOut )
01310 {
01311 short unsigned int succcoord[NUMOFLINKS];
01312 double angles[NUMOFLINKS];
01313 short unsigned int endeffx, endeffy;
01314 int i, inc;
01315
01316 if(fOut == NULL)
01317 fOut = stdout;
01318
01319 EnvROBARMHashEntry_t* HashEntry = EnvROBARM.StateID2CoordTable[SourceStateID];
01320
01321
01322 for(i = 0; i < NUMOFLINKS; i++)
01323 succcoord[i] = HashEntry->coord[i];
01324
01325
01326 for (i = 0; i < NUMOFLINKS; i++)
01327 {
01328
01329 for(inc = -1; inc < 2; inc = inc+2)
01330 {
01331 if(inc == -1)
01332 {
01333 if(HashEntry->coord[i] == 0)
01334 succcoord[i] = EnvROBARMCfg.anglevals[i]-1;
01335 else
01336 succcoord[i] = HashEntry->coord[i] + inc;
01337 }
01338 else
01339 {
01340 succcoord[i] = (HashEntry->coord[i] + inc)%
01341 EnvROBARMCfg.anglevals[i];
01342 }
01343
01344
01345 if(!IsValidCoord(succcoord))
01346 continue;
01347
01348 ComputeContAngles(succcoord, angles);
01349 ComputeEndEffectorPos(angles, &endeffx, &endeffy);
01350 if(endeffx == EnvROBARMCfg.EndEffGoalX_c && endeffy == EnvROBARMCfg.EndEffGoalY_c)
01351 {
01352 if(cost(HashEntry->coord,succcoord) == costtogoal || costtogoal == -1)
01353 {
01354 if(bVerbose)
01355 SBPL_FPRINTF(fOut, "the state is a goal state\n");
01356 printangles(fOut, succcoord, true, bVerbose, bLocal);
01357 return;
01358 }
01359 }
01360 }
01361
01362
01363 succcoord[i] = HashEntry->coord[i];
01364 }
01365
01366 }
01367
01368
01369 void EnvironmentROBARM::PrintEnv_Config(FILE* fOut)
01370 {
01371
01372
01373
01374 SBPL_ERROR("ERROR in EnvROBARM... function: PrintEnv_Config is undefined\n");
01375 throw new SBPL_Exception();
01376
01377 }
01378
01379 void EnvironmentROBARM::PrintHeader(FILE* fOut)
01380 {
01381 SBPL_FPRINTF(fOut, "%d\n", NUMOFLINKS);
01382 for(int i = 0; i < NUMOFLINKS; i++)
01383 SBPL_FPRINTF(fOut, "%.3f ", EnvROBARMCfg.LinkLength_m[i]);
01384 SBPL_FPRINTF(fOut, "\n");
01385 }
01386
01387
01388 void EnvironmentROBARM::SetAllActionsandAllOutcomes(CMDPSTATE* state)
01389 {
01390
01391
01392 SBPL_ERROR("ERROR in EnvROBARM..function: SetAllActionsandOutcomes is undefined\n");
01393 throw new SBPL_Exception();
01394 }
01395
01396
01397 void EnvironmentROBARM::GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV)
01398 {
01399 int i, inc;
01400 short unsigned int succcoord[NUMOFLINKS];
01401 double angles[NUMOFLINKS];
01402
01403
01404 SuccIDV->clear();
01405 CostV->clear();
01406
01407
01408 if(SourceStateID == EnvROBARM.goalHashEntry->stateID)
01409 return;
01410
01411
01412 EnvROBARMHashEntry_t* HashEntry = EnvROBARM.StateID2CoordTable[SourceStateID];
01413
01414
01415
01416 for(i = 0; i < NUMOFLINKS; i++)
01417 succcoord[i] = HashEntry->coord[i];
01418
01419
01420 for (i = 0; i < NUMOFLINKS; i++)
01421 {
01422
01423 for(inc = -1; inc < 2; inc = inc+2)
01424 {
01425 if(inc == -1)
01426 {
01427 if(HashEntry->coord[i] == 0)
01428 succcoord[i] = EnvROBARMCfg.anglevals[i]-1;
01429 else
01430 succcoord[i] = HashEntry->coord[i] + inc;
01431 }
01432 else
01433 {
01434 succcoord[i] = (HashEntry->coord[i] + inc)%
01435 EnvROBARMCfg.anglevals[i];
01436 }
01437
01438
01439 if(!IsValidCoord(succcoord))
01440 continue;
01441
01442
01443 EnvROBARMHashEntry_t* OutHashEntry;
01444 bool bSuccisGoal = false;
01445 short unsigned int endeffx = 0, endeffy = 0;
01446 bool bEndEffComputed = false;
01447 if(abs(HashEntry->endeffx - EnvROBARMCfg.EndEffGoalX_c) < 3 || abs(HashEntry->endeffy - EnvROBARMCfg.EndEffGoalY_c) < 3)
01448 {
01449
01450 ComputeContAngles(succcoord, angles);
01451 ComputeEndEffectorPos(angles, &endeffx, &endeffy);
01452 if(endeffx == EnvROBARMCfg.EndEffGoalX_c && endeffy == EnvROBARMCfg.EndEffGoalY_c)
01453 {
01454 bSuccisGoal = true;
01455
01456 }
01457 bEndEffComputed = true;
01458 }
01459
01460 if((OutHashEntry = GetHashEntry(succcoord, NUMOFLINKS, bSuccisGoal)) == NULL)
01461 {
01462 if(bEndEffComputed == false)
01463 {
01464 ComputeContAngles(succcoord, angles);
01465 ComputeEndEffectorPos(angles, &endeffx, &endeffy);
01466 }
01467
01468
01469 OutHashEntry = CreateNewHashEntry(succcoord, NUMOFLINKS, endeffx, endeffy);
01470 }
01471 SuccIDV->push_back(OutHashEntry->stateID);
01472 CostV->push_back(cost(HashEntry->coord,succcoord));
01473 }
01474
01475
01476 succcoord[i] = HashEntry->coord[i];
01477 }
01478 }
01479
01480 void EnvironmentROBARM::GetPreds(int TargetStateID, vector<int>* PredIDV, vector<int>* CostV)
01481 {
01482
01483 SBPL_ERROR("ERROR in EnvROBARM... function: GetPreds is undefined\n");
01484 throw new SBPL_Exception();
01485 }
01486
01487
01488
01489 void EnvironmentROBARM::GetRandomSuccsatDistance(int SourceStateID, std::vector<int>* SuccIDV, std::vector<int>* CLowV)
01490 {
01491 short unsigned int coord[NUMOFLINKS];
01492 short unsigned int endeffx, endeffy;
01493 double angles[NUMOFLINKS];
01494
01495
01496 SuccIDV->clear();
01497 CLowV->clear();
01498
01499
01500 int numofsuccs = ROBARM_NUMOFRANDSUCCSATDIST;
01501
01502
01503 if(SourceStateID == EnvROBARM.goalHashEntry->stateID)
01504 return;
01505
01506
01507 EnvROBARMHashEntry_t* HashEntry = EnvROBARM.StateID2CoordTable[SourceStateID];
01508
01509
01510 for (int succind = 0; succind < numofsuccs; succind++)
01511 {
01512
01513 int maxcoordind = (int)(NUMOFLINKS*(((double)rand())/RAND_MAX));
01514
01515
01516 for (int cind = 0; cind < NUMOFLINKS; cind++)
01517 {
01518
01519 if(cind == maxcoordind)
01520 {
01521
01522 if((((double)rand())/RAND_MAX) > 0.5)
01523 {
01524
01525 coord[cind] = (HashEntry->coord[cind] + ROBARM_LONGACTIONDIST_CELLS)%EnvROBARMCfg.anglevals[cind];
01526 }
01527 else
01528 {
01529
01530 if(HashEntry->coord[cind] < ROBARM_LONGACTIONDIST_CELLS)
01531 coord[cind] = HashEntry->coord[cind] + EnvROBARMCfg.anglevals[cind] - ROBARM_LONGACTIONDIST_CELLS;
01532 else
01533 coord[cind] = HashEntry->coord[cind] - ROBARM_LONGACTIONDIST_CELLS;
01534
01535
01536
01537
01538
01539 }
01540 }
01541 else
01542 {
01543
01544 int offset = (int)(ROBARM_LONGACTIONDIST_CELLS*(((double)rand())/RAND_MAX));
01545 if((((double)rand())/RAND_MAX) > 0.5)
01546 offset = -offset;
01547
01548
01549 if(offset >= 0)
01550 {
01551
01552 coord[cind] = (HashEntry->coord[cind] + offset)%EnvROBARMCfg.anglevals[cind];
01553 }
01554 else
01555 {
01556
01557 coord[cind] = (HashEntry->coord[cind] + offset);
01558 if(HashEntry->coord[cind] < -offset)
01559 coord[cind] = HashEntry->coord[cind] + EnvROBARMCfg.anglevals[cind] + offset;
01560
01561
01562
01563
01564
01565 }
01566 }
01567 }
01568
01569
01570
01571
01572 if(!IsValidCoord(coord))
01573 continue;
01574
01575
01576
01577
01578
01579 ComputeContAngles(coord, angles);
01580 ComputeEndEffectorPos(angles, &endeffx, &endeffy);
01581
01582
01583 if(abs(HashEntry->endeffx - endeffx) > ROBARM_LONGACTIONDIST_CELLS || abs(HashEntry->endeffy - endeffy) > ROBARM_LONGACTIONDIST_CELLS)
01584 continue;
01585
01586
01587 bool bIsGoal = false;
01588 if(endeffx == EnvROBARMCfg.EndEffGoalX_c && endeffy == EnvROBARMCfg.EndEffGoalY_c)
01589 {
01590 bIsGoal = true;
01591
01592 }
01593
01594 EnvROBARMHashEntry_t* OutHashEntry = NULL;
01595 if((OutHashEntry = GetHashEntry(coord, NUMOFLINKS, bIsGoal)) == NULL)
01596 {
01597
01598 OutHashEntry = CreateNewHashEntry(coord, NUMOFLINKS, endeffx, endeffy);
01599 }
01600
01601
01602 int clow;
01603 clow = EnvironmentROBARM::GetFromToHeuristic(HashEntry->stateID, OutHashEntry->stateID);
01604
01605 SuccIDV->push_back(OutHashEntry->stateID);
01606 CLowV->push_back(clow);
01607
01608
01609 }
01610
01611
01612 if(abs(EnvROBARMCfg.EndEffGoalX_c - HashEntry->endeffx) <= ROBARM_LONGACTIONDIST_CELLS &&
01613 abs(EnvROBARMCfg.EndEffGoalY_c - HashEntry->endeffy) <= ROBARM_LONGACTIONDIST_CELLS)
01614 {
01615
01616 EnvROBARMHashEntry_t* OutHashEntry = EnvROBARM.goalHashEntry;
01617
01618 int clow = EnvironmentROBARM::GetFromToHeuristic(HashEntry->stateID, OutHashEntry->stateID);
01619
01620 SuccIDV->push_back(OutHashEntry->stateID);
01621 CLowV->push_back(clow);
01622
01623 }
01624
01625 }
01626
01627
01628 int EnvironmentROBARM::GetEdgeCost(int FromStateID, int ToStateID)
01629 {
01630
01631 #if DEBUG
01632 if(FromStateID >= (int)EnvROBARM.StateID2CoordTable.size()
01633 || ToStateID >= (int)EnvROBARM.StateID2CoordTable.size())
01634 {
01635 SBPL_ERROR("ERROR in EnvROBARM... function: stateID illegal\n");
01636 throw new SBPL_Exception();
01637 }
01638 #endif
01639
01640
01641 EnvROBARMHashEntry_t* FromHashEntry = EnvROBARM.StateID2CoordTable[FromStateID];
01642 EnvROBARMHashEntry_t* ToHashEntry = EnvROBARM.StateID2CoordTable[ToStateID];
01643
01644
01645 return cost(FromHashEntry->coord, ToHashEntry->coord);
01646
01647 }
01648
01649 int EnvironmentROBARM::GetRandomState()
01650 {
01651 short unsigned int coord[NUMOFLINKS];
01652 double angles[NUMOFLINKS];
01653 short unsigned int endeffx, endeffy;
01654 EnvROBARMHashEntry_t* HashEntry = NULL;
01655
01656 SBPL_PRINTF("picking a random state...\n");
01657 while(1)
01658 {
01659
01660
01661 for(int i = 0; i < NUMOFLINKS; i++)
01662 {
01663
01664 coord[i] = (short unsigned int)(EnvROBARMCfg.anglevals[i]*(((double)rand())/(((double)RAND_MAX)+1)));
01665 }
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01678
01679
01680
01681
01682
01683
01684
01685
01686
01687
01688
01689
01690
01691
01692
01693
01694
01695
01696
01697
01698
01699
01700
01701
01702
01703
01704
01705
01706
01707
01708
01709
01710
01711
01712
01713
01714
01715
01716
01717
01718
01719
01720
01721
01722 if(!IsValidCoord(coord))
01723 {
01724
01725 continue;
01726 }
01727 else
01728
01729 break;
01730 }
01731 SBPL_PRINTF("done\n");
01732
01733
01734 ComputeContAngles(coord, angles);
01735 ComputeEndEffectorPos(angles, &endeffx, &endeffy);
01736 bool bIsGoal = false;
01737 if(endeffx == EnvROBARMCfg.EndEffGoalX_c && endeffy == EnvROBARMCfg.EndEffGoalY_c)
01738 {
01739 bIsGoal = true;
01740
01741 }
01742
01743 if((HashEntry = GetHashEntry(coord, NUMOFLINKS, bIsGoal)) == NULL)
01744 {
01745
01746 HashEntry = CreateNewHashEntry(coord, NUMOFLINKS, endeffx, endeffy);
01747 }
01748
01749
01750 return HashEntry->stateID;
01751 }
01752
01753 bool EnvironmentROBARM::AreEquivalent(int State1ID, int State2ID)
01754 {
01755
01756 EnvROBARMHashEntry_t* HashEntry1 = EnvROBARM.StateID2CoordTable[State1ID];
01757 EnvROBARMHashEntry_t* HashEntry2 = EnvROBARM.StateID2CoordTable[State2ID];
01758
01759 return (HashEntry1->endeffx == HashEntry2->endeffx && HashEntry1->endeffy == HashEntry2->endeffy);
01760
01761 }
01762
01763
01764