Merge branch 'master' of benesp7@rtime.felk.cvut.cz:/var/git/eurobot

diff --git a/src/motion/spline.cc b/src/motion/spline.cc
index 67d967eb261a5c55fbc42790a4f156f8695d62bd..6714da036401531464b9eca67bb9732fe5ffb723 100644 (file)
--- a/src/motion/spline.cc
+++ b/src/motion/spline.cc
@@ -16,8 +16,14 @@ namespace Segment {
     Spline::Spline(Point *_p1, Point *_p2, Point *_corner ) :
        p1(_p1), p2(_p2), corner(_corner)
     {
-        const double A_SHAPE = 6860;   // constants for shaping a clothoid like spline (derived by heuristics TODO: describe the source)
+        // constants for shaping a clothoid like spline 
+        // (derived by heuristics: see Matlab file 'splines/optimalm.m')
+        const double A_SHAPE = 6860;   
         const double B_SHAPE = 4.4;
+        // (derived by heuristics: see Matlab file 'splines/optimalm2.m')
+        const double C_SHAPE = 0.0423;
+        const double D_SHAPE = 0.008;
+
         param0 = 0;
         param1 = 1;
 
@@ -32,12 +38,11 @@ namespace Segment {
         gamma = fabs(angle1 - angle2);
         if (gamma > M_PI)
             gamma =  (2*M_PI) - gamma; // !!!
-        if (gamma < to_rad(7.0))
+        if (gamma < to_rad(10.0))
             {
-                   // Heurictics - TODO: link to matlab
-                   m = 0.0423 * to_deg(gamma) + 0.008;
-                   // FIXME: Multiply by distance
-//            printf("---- too small curve %lf - m=%lf ", gammaDeg, m);
+            // constants for shaping a clothoid like spline, more accuratly for small angles
+                   m = C_SHAPE * to_deg(gamma) + D_SHAPE;
+            m = m * distance;
             }
         else
             {
@@ -47,7 +52,7 @@ namespace Segment {
             }
 
 
-        // Calculate polynom constants (TODO: link to m-file)
+        // Calculate polynom constants (see 'splines/splines.m')
         double x0 = p1->x;
         double y0 = p1->y;
         double x1 = p2->x;
@@ -97,7 +102,6 @@ namespace Segment {
  * Finds maximal speeds vc, v1 and v2 (in the middle, in the beginning and at the end).
  * The speed profile looks like a letter 'v' because of clothoid's curvature profile shape.
  * This is a simplified approach, but quick. Other constraints play a significant role.
- * @TODO better acquaint with the constraint meaning
  * @TODO display constraints with on-line speed and accs values
  */
     void Spline::setMaxV(const TrajectoryConstraints &constr)
@@ -114,14 +118,20 @@ namespace Segment {
             }
         minr = fabs(getRadiusAtParam(minrParam));
 
+        // vc ... speed in the middle of the spline
         vc = constr.maxv;
+        // angular speed depends on radius
         vc = fmin(vc, constr.maxomega * minr);
-            // angular speed depends on radius
         vc = fmin(vc, sqrt(constr.maxcenacc * minr));
 
-       // TODO: How was this derived? Check units! 
-        vc = fmin(vc, constr.maxangacc*length/2*minr);
-            // vc ... speed in the middle of the spline
+        // angacc = dAngspeed / dTime = speed^2 / (dRadius*dDistance)
+        // speed^2 = angacc * dRadius * dDistance
+        // speed = sqrt (angacc * dRadius * dDistance)
+        // where: dRadius * dDistance = 1 / (dCurvature/dDistance) = konst. (clothoid)
+        // dCurvature/dDistance = maxCurvature / (length/2) = 2 / (minRadius*length)
+
+        vc = fmin(vc, sqrt( constr.maxangacc * (length/2.0*minr) ));
+
 
 #ifdef MATLAB_MEX_FILE
         char str [200];
@@ -317,14 +327,12 @@ namespace Segment {
         getPointAt(-distance, newEnd);
         if (distance > 0) {    // end being cut off
             p2 = newEnd;
-            param1 -= (param1-param0)*distance/length; // FIXME: Use dist2param()
-            // (param1 - newParam1) / (param1-param0) == distance/length
+            param1 -= dist2param(distance);
 
         } else {               // begining being cut off
             p1 = newEnd;
             distance = -distance;
-            param0 += (param1-param0)*distance/length; // FIXME: Use dist2param()
-            // (newParam0 - param0) / (param1-param0) == distance/length
+            param0 += dist2param(distance);
         }
         length -= distance;
     }

diff --git a/src/motion/splines/optimalm.m b/src/motion/splines/optimalm.m
index 93735e97f3185f55d3336461feb21b2453f6d0d2..d11e309e5153ee3130fe9019bacc91544bebe2c9 100644 (file)
--- a/src/motion/splines/optimalm.m
+++ b/src/motion/splines/optimalm.m
@@ -1,5 +1,5 @@
 clear all; close all; clc;\r
-angles = 10:10:350;\r
+angles = 10:10:20;\r
 m=zeros(size(angles));\r
 for i=1:length(angles),\r
     a=angles(i);\r
@@ -16,4 +16,4 @@ hold on
 B=4.4;\r
 A=6850;\r
 eli = sqrt(B-(angles-180).^2./A);\r
-plot(angles, eli, 'r');
\ No newline at end of file
+plot(angles, eli, 'r');\r

diff --git a/src/motion/splines/optimalm2.m b/src/motion/splines/optimalm2.m
new file mode 100644 (file)
index 0000000..90c9e05
--- /dev/null
+++ b/src/motion/splines/optimalm2.m
@@ -0,0 +1,19 @@
+clear all; close all; clc;\r
+angles = 1:1:10;\r
+m=zeros(size(angles));\r
+for i=1:length(angles),\r
+    a=angles(i);\r
+    m(i) = fminsearch(@(m)plotspline(0,a,1,m), 1);\r
+    figure(2);\r
+    plot(angles,m);\r
+    grid\r
+    title('Optimal value of m');\r
+    xlabel('Angle [deg]');\r
+    ylabel('m');\r
+end\r
+\r
+hold on\r
+C=0.0423;\r
+D=0.008;\r
+eli = C * angles + D;\r
+plot(angles, eli, 'r');\r

diff --git a/src/motion/trajectory.cc b/src/motion/trajectory.cc
index 7b0385e0d4b4052fee88634296ed6662d40e6fdd..d44a19ea751f563bc6120fb53c8f6266e653a7f4 100644 (file)
--- a/src/motion/trajectory.cc
+++ b/src/motion/trajectory.cc
@@ -92,7 +92,7 @@ Trajectory::points2segments()
         edge = fabs((*p2)->angleTo(**p1) - (*p2)->angleTo(**p3));
         if (edge > M_PI)
             edge =  (2*M_PI) - edge;
-        if (edge*180/M_PI < 15.0)
+        if (edge*180/M_PI < 3.0)
             {
 //            printf("* - * - * point added due to %lf angle\n", edge/M_PI*180);
             edge = (*p2)->angleTo(**p1) + M_PI/2.0;
@@ -589,7 +589,6 @@ void Trajectory::addTurns()
  *
  * @param _initPos The point where the trajectory should
  * start. Typically the current robot position.
- * @todo Solve properly the case, when initPos has non-zero speed.
  * @return True in case of success, false when the trajectory has only
  * one point and initPos is already equal to that point.
  */

diff --git a/src/pathplan/aalgorithm.c b/src/pathplan/aalgorithm.c
index 0fdb9d552f15560e838db72e80bf63ab181758ad..50f4673d639881aa9568165973569870eaca8e85 100644 (file)
--- a/src/pathplan/aalgorithm.c
+++ b/src/pathplan/aalgorithm.c
@@ -146,6 +146,32 @@ int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double
        start->backpointer = start;
         pushNodeInOrder(start);
 
+    ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
+        const struct { int x; int y; } neigh[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
+              {-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}, { 2, 0}, { 0, 2}, {-2, 0}, {0, -2}};
+
+
+    // Adds safety zone around the obstacle.
+    // This zone contains one neighbour cell (to avoid frequent recalculation).
+    int xx, yy, ii;
+    for (yy=0;yy<MAP_HEIGHT;yy++) {
+        for (xx=0;xx<MAP_WIDTH;xx++) {
+            // check obstacles around this cell to determine safety zone flag
+            map->cells[yy][xx].flags &= ~MAP_FLAG_PLAN_MARGIN;  // delete margin status
+            for (ii=0; ii< sizeof(neigh)/sizeof(neigh[0]); ii++)
+            {
+                if (ShmapIsCellInMap(xx+neigh[ii].x, yy+neigh[ii].y) 
+                    && ((map->cells[yy+neigh[ii].y][xx+neigh[ii].x]).flags & MAP_FLAG_DET_OBST))
+                {
+                    // creates safety status
+                    map->cells[yy][xx].flags |= MAP_FLAG_PLAN_MARGIN;
+                    break;
+                }
+            }
+        }
+    }
+
+
        /// <li> REPEAT until the queue is empty.
 
        do {
@@ -171,9 +197,10 @@ int aAlgorithm(double xstart_real,double ystart_real, double xgoal_real, double
                         break;
                }
 
-               ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
-                const struct { int x; int y; } neighbours[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
+    ///  <li>  Expand nbest :for all x E Star(nbest) that are not in C
+        const struct { int x; int y; } neighbours[] = { { 0, 1}, { 1, 1}, { 1, 0}, {-1, 1},
                                                           {-1, 0}, {-1,-1}, { 0,-1}, { 1,-1}};
+
                for (i=0; i< sizeof(neighbours)/sizeof(neighbours[0]); i++) {
                         xcontig = x + neighbours[i].x;
                         ycontig = y + neighbours[i].y;
@@ -266,6 +293,7 @@ float cFunction(int x1, int y1, int x2, int y2)
 
 }
 
+
 /**
  * 
  * @brief Calculates Map Heuristic values. 

diff --git a/src/pathplan/map.c b/src/pathplan/map.c
index 1d38c92e80d09f6248cbded0c0bc19a6a1e42062..b0c7d64ec431d8cd65e0e18e2403cb98b5465cfe 100644 (file)
--- a/src/pathplan/map.c
+++ b/src/pathplan/map.c
@@ -153,7 +153,7 @@ int ShmapIsFreeCell(int x, int y)
         struct map_cell *cell;
         if(map && ShmapIsCellInMap(x,y)) {        
                 cell = &(map->cells[y][x]);
-                return (cell->detected_obstacle == 0) && ((cell->flags & (MAP_FLAG_WALL|MAP_FLAG_DET_OBST)) == 0);
+                return (cell->detected_obstacle == 0) && ((cell->flags & (MAP_FLAG_WALL|MAP_FLAG_DET_OBST|MAP_FLAG_PLAN_MARGIN)) == 0);
         }
         else return -1;
 }

diff --git a/src/robofsm/fsmmove.cc b/src/robofsm/fsmmove.cc
index 4bcfd6a8637d4b5af2cd7d5fbc7aec18fd423efa..d3242e13eb341df2dfae3040b1f1f9128ae72610 100644 (file)
--- a/src/robofsm/fsmmove.cc
+++ b/src/robofsm/fsmmove.cc
@@ -115,7 +115,7 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
        bool valid;
        unsigned i;
 //     const double times[] = { 0.5, 0.3, 0.1 }; // seconds
-       const double times[] = { 0.5, 0.4, 0.3 }; // seconds
+       const double times[] = { 0.7, 0.5, 0.4, 0.3 }; // seconds
 
 
        for (i=0; i < sizeof(times)/sizeof(times[0]); i++) {