AHS Extensions -> Coding

1. Just to remember. In my file structure, the planner is separated in three subdirectories.
1. mgp. This one includes the implementation of NAIVE and GREEDY algorithms for the Multi-Goal Planner.
2. shrink. Comprises the implementation of the planner using the shrunken version of the robot. Under development.
3. mpk. This has basically the original MPK implementation, which includes SBL planner. I also added Basic PRM and the AHS implementations. So, under directory prm we have:
   
1. Single Sampler. Corresponds to the Basic PRM.
2. Multi-Goal PRM. Corresponds to Requicha strategy for MG Path planning.
3. AS1: Competing against best fixed weight strategy.
4. AS2: Cost sensitive. U+G+RBB parameterized.
5. AS3: Variable length history.
2. I will work now on the inclusion of visibility-based PRM, so I can run some experiments and make a comparison.
   

Some useful commands

1. $ enscript -2Gr prmPlanner.H -o prmPlanner.ps
2. Search for a string in the files:
   $ grep -l 'DistGaussSample*' *.C
3. At command:
   $ at 1200 [ENTER] command CTRL-D
4. Counting occurrences:
   $ grep word file.txt |wc -l
5. Counting the number of lines in a directory:
   wc -l *
6. The installation of boost has some problems related with the disjoint sets implementation. Adding
   #include boost/pending/disjoint_sets.hpp in the file
   /graph/detail/incremental_components.hpp> solves the issue.
7. Some old codes complain about CLK_TCK:
   #ifndef CLK_TCK
   #define CLK_TCK CLOCKS_PER_SEC
   #endif
   The definition is not anymore in /usr/include/sys/time.h but on
   /usr/include/time.h
   

Testing Collision Checker

1. Testing the influence of the the parameter that defines the minimum size of the spheres in the functioning of the collision checker. Using
   double SphereColModel::_maxCoverSphereRadius=0.0001; made the PC to run out of memory.
   0.001: more than 10 minutes just to create the hierarchies.
   0.01: about 8 seconds in building the hierarchy. The environment used is the one with the fanuc robot with the lathe and the press.
   
2. It is working now. My guess is that given that the environment that I was using had the plate (a very thin one), using big spheres was provoking to detect incorrectly some collisions. The truth is that changing the value of _maxCoverSphereRadius from 1 to 0.01 did result in the planner working now. I am happy and I now deserve a good meal :P
3. I have created a backup of the shrink, shrinkMPK and SphereColChecker under Backs directory. Just in case. The version of today basically consist of the planner working with Quinlan's collision checker instead of PQP.
4. I will spend the rest of the afternoon doing two things:
   a) Checking the ICRA presentation.
   b) Trying to come up with an idea on how to modify the construction and search of the hierarchy in the collision checker.
5. mount -t vfat /dev/sda2 /mnt/ipod
   gtkpod
   --> mpeg4ip, libid3tag, gtk
6. Kernel updated in the laptop. The new one (2.6.11) comes with a new version of the gcc compiler (3.4.2-6)
   

Bug in SphereColChecker

1. David pointed out a bug on the original Quinlan's code:
   

   On line 63 of gilbert.c, change:
             if (dstsq >= lastdstsq || nvs == 4) {
   to
             if (dstsq >= lastdstsq * (1 - 1e-10) || nvs == 4) {

   Once I changed that, I don't get the computer in an infinite loop.
   
2. Making sure that the change on the collision checker did not affected anything else. Trying the planner with different problems I get always that one of the configurations (start or goal) are in collision. My guess is that it has to do with the translation of the base of the robot. Of course, the image seems ok, but the collision checking is detecting collisions incorrectly. I will check that the transforms are correctly applied to the models.
3. Somethings to remember.
   a) I am not prunning now pairs. Which means that every time the collision checker is called all possible pairs are checked (in the worst case). This is due to the fact that in the original version of MPK, Bounding Volumes (BV) are constructed using information extracted from PQP and then the BVs are used both for defining the colliding pairs as well as in the collision checking in a step done before calling the actual collision checker.
   b) I have two models of the geometry of the obstacles. The same applies for the robot links. This is because the GUI requires the BV for every link. I will check later how to extract that information using only SphereColChecker.
   
4. mpkBVControl::init(robots, obstacles, bv_level);
   mpkGUI::scene->addChild(mpkBVControl::ivroot());
   is producing a segmentation fault....
   
5. Problem corrected. But now I cannot pick the links of the robot in the GUI. That is not good, since it may be due to some problem while reading the robot description. However, I cannot identify where is the problem.
6. The Bounding Boxes are correctly computed now.
   

Problem isolated

Well. Now I know that the problem is here:

Sdist3 result = scollide3(pA->GetSObject(), pA->GetFrame(), pB->GetSObject(), pB->GetFrame());
When this function is called from mpkCollDistAlgo::Collision(), the control never returns. I have checked the pointers of the objects received. No problem there. I have tested with different scenes. No problem there. I have two candidates now to be the problem:
a) The geometry of the model (i.e., the triangles obtained with the callBack in OpenInventor).
b) Function scollide3, in Quinlan's code.

The first is easy to check. The second would imply checking the whole original Quinlan's Collision Checker. Arrgggg !

If I cannot solve this by this afternoon I will check what can I do with PQP instead and propose that to David. I cannot loose more time in this hacking. Is not productive and I feel desperate that I cannot even think in other more interesting problems.

Ok. I checked that. The model sent is correct as well as the frames associated to them. Quinlan's code gets trapped at some point inside the search of the hierarchy. I don't think it would be a good idea to go deep in this direction. It will consume more time that what has already taken. I need to suggest something else.

One possibility would be to use the hierarchy that is built in MPK using the bounding boxes. I don't know now the details.

Collision Checker

1. I checked my former version of the planner (SBL). I didn't remember that such version requires ViewKit to visualize the environments. I don't think I have the rpms and I know that they are not around anymore. And even in the case where they were, I don't think is a good idea to develop something based on those libraries. So, my only one option is to find out what is happening with the collision checker call and correct it. So far, I know that the problem does not have to do with the SphereCollisionChecker. I created other objects inside the Collision() function and they are correctly created, and the functions ComputeDistance and CheckCollision() work fine. So, the problem is with the models. Either the geometrical definition (i.e., the triangles), or with the position/orientation.
2. I have checked the call to the SphereColChecker. If I create the models inside the function, update the info on the position/orientation and call the ComputeDistance or Collision() functions, then it works perfectly. My guess is that it has something to do with the pointers. Remember that SphereColChecker was originaly developed in C. I will check that now.
   

Preparing the planner

I have invested a lot of time trying to modify MPK to use SphereColChecker instead of PQP. This part of the work is a nightmare. Most of the classes are under directory /basic which contains about 5500 lines of code. Functions are interrelated and most of them are kind of 'hardwired' with PQP. If I cannot succed in this week I will better try to modify my old planner code. At least I remember better where are the things. And eventhough my code is not as elegant and efficient as MPK, I need to produce results soon.

As for today, the things are as follows:
* fmstudio calls the planner (let's suppose for now that it is sbl). The planner requieres a free milestone, so, it calls the sampler and the checker. This last part in the one tha concerns me now. In order to check one configuration, the planner creates an object of class mpkSimpleSegment. This object calls function iteration_step(). That function uses an object of the class mpkConfigChecker. The object calls function collision(). This function makes a call to mpkCollDistAlgo::Collision().
* The function mpkCollDistAlgo::Collision() first updates the translation/rotation info on the objects to be checked. Then, it makes a call to the actual low level collision checker. This call, of course depends on the collision checker in use. In my case, now, this function is:
SphereColChecker chk;

ColVector3 t1, t2;
ColMatrix3 r1, r2;

// Update the position/orientation of the objects
for(int i=0;i<3;i++){
t1[i]=T1[i];
t2[i]=T2[i];
}
for(int i=0;i<3;i++)
for(int j=0;j<3;j++){
r1[i][j]=R1[i][j];
r2[i][j]=R2[i][j];
}

o1->SetTransform(r1,t1);
o2->SetTransform(r2,t2);
return chk.CheckCollision(*o1,*o2);

However, the computer gets stuck in the call to CheckCollision(). Checking the values of the position/rotation information, they seem to be ok. So, the problem is either on the models that I am passing to the SphereCollisionChecker (SCC) or on the library. For obvious reasons, I would go to check first the models.