added the stressTest, also updated robotCommander
Now with intrinsic parameters
working on model
 Added in the p2m, getRadius and getActualHeight
Added the safety zone
the CModel01 is now correct