src/motion/spline.cc

   1 //     Copyright 2009 Petr Beneš
   2 //     Copyright 2009 Michal Sojka <sojkam1@fel.cvut.cz>
   3 //
   4 //     This file is part of Trgen library.
   5 //
   6 //     Trgen is free software: you can redistribute it and/or modify
   7 //     it under the terms of the GNU General Public License as published by
   8 //     the Free Software Foundation, either version 3 of the License, or
   9 //     (at your option) any later version.
  10 //
  11 //     Trgen is distributed in the hope that it will be useful,
  12 //     but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 //     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 //     GNU General Public License for more details.
  15 //
  16 //     You should have received a copy of the GNU General Public License
  17 //     along with Trgen.  If not, see <http://www.gnu.org/licenses/>.
  18
  19 #include "trgen.h"
  20 #include "trgendbg.h"
  21 #include <stdlib.h>
  22 #include <stdio.h>
  23
  24 static inline double to_deg(double angle) {
  25         return angle/M_PI*180;
  26 }
  27
  28 static inline double to_rad(double angle) {
  29         return angle/180.0*M_PI;
  30 }
  31
  32
  33 namespace Segment {
  34
  35     Spline::Spline(Point *_p1, Point *_p2, Point *_corner ) :
  36         p1(_p1), p2(_p2), corner(_corner)
  37         {
  38         // constants for shaping a clothoid like spline
  39         // (derived by heuristics: see Matlab file 'splines/optimalm.m')
  40         const double A_SHAPE = 6860;
  41         const double B_SHAPE = 4.4;
  42         // (derived by heuristics: see Matlab file 'splines/optimalm2.m')
  43         const double C_SHAPE = 0.0423;
  44         const double D_SHAPE = 0.008;
  45
  46         param0 = 0.0;
  47         param1 = 1.0;
  48
  49         // Calculate distance
  50         distance = p1->distanceTo(*corner);
  51         if (fabs(p2->distanceTo(*corner) - distance) < 1e-3)
  52             dbgPrintf("Error: distances must be equal!");
  53
  54         // Calculate central angle
  55         double angle1 = corner->angleTo(*p1);
  56         double angle2 = corner->angleTo(*p2);
  57         gamma = fabs(angle1 - angle2);
  58         if (gamma > M_PI)
  59             gamma =  (2*M_PI) - gamma;
  60         if (gamma < to_rad(10.0))
  61             {
  62             // constants for shaping a clothoid like spline, more accurately for small angles
  63             m = C_SHAPE * to_deg(gamma) + D_SHAPE;
  64             m = m * distance;
  65             }
  66         else
  67             {
  68             // Count vectors to shape splnine like a clothoid using eliptic approximation of relation
  69             m = sqrt(B_SHAPE - pow(to_deg(gamma-M_PI), 2)/A_SHAPE);     // experimental result
  70             m = m * distance;
  71             }
  72
  73
  74         // Calculate polynom constants (see 'splines/splines.m')
  75         double x0 = p1->x;
  76         double y0 = p1->y;
  77         double x1 = p2->x;
  78         double y1 = p2->y;
  79         double xx0 = m*(-cos(angle1));
  80         double yy0 = m*(-sin(angle1));
  81         double xx1 = m*(cos(angle2));
  82         double yy1 = m*(sin(angle2));
  83
  84         Ax =   -3*xx1-3*xx0-6*x0+6*x1;
  85         Bx =  7*xx1+8*xx0+15*x0-15*x1;
  86         Cx = -4*xx1-6*xx0-10*x0+10*x1;
  87         Dx =                        0;
  88         Ex =                      xx0;
  89         Fx =                       x0;
  90
  91         Ay =   -3*yy1-3*yy0-6*y0+6*y1;
  92         By =  7*yy1+8*yy0+15*y0-15*y1;
  93         Cy = -4*yy1-6*yy0-10*y0+10*y1;
  94         Dy =                        0;
  95         Ey =                      yy0;
  96         Fy =                       y0;
  97
  98
  99         // Calculate length (using numeric integration, there is no better way)
 100         const int SENS = 100; // sensitivity of numeric integration
 101         Point pp1 = Point(0, 0);
 102         Point pp2 = Point(0, 0);
 103         getPointAtParam(param0, &pp2);
 104         length = 0;
 105         for (int i = 1; i <= SENS; i++) {
 106             double par = (double)i/SENS*(param1-param0);
 107             double dl;
 108             pp1.x = pp2.x;
 109             pp1.y = pp2.y;
 110             getPointAtParam(par, &pp2);
 111             dl = pp1.distanceTo(pp2);
 112             length += dl;
 113         }
 114
 115 }
 116
 117
 118 /**
 119  * Finds maximal speeds vc, v1 and v2 (in the middle, in the beginning and at the end).
 120  * The speed profile looks like a letter 'v' because of clothoid's curvature profile shape.
 121  * This is a simplified approach, but quick. Other constraints play a significant role.
 122  * @TODO display constraints with on-line speed and accs values
 123  */
 124     void Spline::setMaxV(const TrajectoryConstraints &constr)
 125     {
 126         double minrParam; // point with the smallest radius of curvature
 127         double minr;      // the radius
 128         if (param0 <= 0.5 && param1 >=0.5)
 129             minrParam = 0.5;
 130         else {
 131             if (param0 > 0.5)
 132                 minrParam = param0;
 133             if (param1 < 0.5)
 134                 minrParam = param1;
 135             }
 136         minr = fabs(getRadiusAtParam(minrParam));
 137
 138         // vc ... speed in the middle of the spline
 139         vc = constr.maxv;
 140         // angular speed depends on radius
 141         vc = fmin(vc, constr.maxomega * minr);
 142         vc = fmin(vc, sqrt(constr.maxcenacc * minr));
 143
 144         // angacc = dAngspeed / dTime = speed^2 / (dRadius*dDistance)
 145         // speed^2 = angacc * dRadius * dDistance
 146         // speed = sqrt (angacc * dRadius * dDistance)
 147         // where: dRadius * dDistance = 1 / (dCurvature/dDistance) = konst. (clothoid)
 148         // dCurvature/dDistance = maxCurvature / (length/2) = 2 / (minRadius*length)
 149
 150         vc = fmin(vc, sqrt( constr.maxangacc * (length/2.0*minr) ));
 151         dbgPrintf("segment %p: vc0=%g\n", this, vc);
 152
 153 #ifdef MATLAB_MEX_FILE
 154         char str [200];
 155         //sprintf (str, "disp('pred %lf, po %lf')", remember, vc);
 156         //mexEvalString(str);
 157 #endif
 158
 159         // Find optimal v1 and v2 by binary chopping
 160         v1 = constr.maxv/2.0;
 161         v2 = constr.maxv/2.0;
 162
 163         double maxvc = vc;
 164         double step = v1 / 2.0;
 165         double stepc = vc / 2.0;
 166
 167         // speed optimalisation
 168         for (int i =0; i<30; i++) {
 169             acc = (v2-vc)*(v2+vc)/length;
 170             acc = fmin(acc, constr.maxacc);
 171
 172             const int PARTS = 10;
 173             int problemPart = 0;
 174             double problemOvershoot;
 175             double actOmega;
 176             // find out where is the biggest overshoot
 177             for (int part = 1; part < PARTS; part++)  {
 178                 problemOvershoot = 0;
 179                 if (part <= PARTS/2)
 180                 {
 181                     actOmega = sqrt(-2*acc*(length*part/PARTS) + pow(v1, 2))  /
 182                         fabs(getRadiusAt(length*part/PARTS));
 183                 }
 184                 else
 185                 {
 186                     actOmega = sqrt(2*acc*(length*part/PARTS - length/2) + pow(vc, 2))  /
 187                         fabs(getRadiusAt(length*part/PARTS));
 188                 }
 189
 190                 if (actOmega > constr.maxomega)
 191                 {
 192                     if (actOmega - constr.maxomega > problemOvershoot)
 193                     {
 194                         problemOvershoot = actOmega - constr.maxomega;
 195                         problemPart = part;
 196                     }
 197                 }
 198             }
 199
 200             // decide which speed parameter of segment change and how
 201             if (problemPart == 0)   // satisfying constraints
 202             {
 203                 v1 += step;
 204                 v2 += step;
 205                 step /= 2;
 206                 vc += stepc;
 207                 vc = fmin (vc, maxvc);
 208                 stepc /= 2;
 209             }
 210             else
 211             {
 212                 if (abs(problemPart - PARTS/2) < PARTS/9 && length > 0.15 && vc > 0.00005)  // vc pull down
 213                 {
 214                     vc -= stepc;
 215                     stepc /= 2;
 216                     v1 *= 0.95;
 217                     v2 *= 0.95;
 218                 }
 219                 else        // v1, v2 pull down
 220                 {
 221                     v1 -= step;
 222                     v2 -= step;
 223
 224                     step /= 2;
 225                     if (v1 < vc)
 226                         vc = v1;
 227                 }
 228             }
 229 /*            if (length/(v1+vc)/2 > 4)
 230             {
 231                 v1 *= length/(v1+vc)/2 /4;
 232                 v2 *= length/(v1+vc)/2 /4;
 233                 vc *= length/(v1+vc)/2 /4;
 234             } */
 235
 236                 if (v1 < 0.001 || v2 < 0.001) {
 237                 v1 = 0.001;
 238                 v2 = 0.001;
 239                 }
 240                 if (vc < 0.001) {
 241                 vc = 0.001;
 242
 243                 }
 244         }
 245
 246
 247
 248
 249         // kontrola
 250 /*        double kkk = 0;
 251         for (double xxx = length/20; xxx<length/2; xxx += length/20)
 252         {
 253             kkk = fmax (sqrt(-2*acc*xxx + pow(v1, 2)) / fabs(getRadiusAt(xxx)), kkk);
 254             kkk = fmax (sqrt(2*acc*xxx + pow(vc, 2)) / fabs(getRadiusAt(xxx+length/2)), kkk);
 255         }
 256         printf("MAX %lf, v1 %lf vc %lf v2 %lf acc %lf len %lf ang %lf\n", kkk, v1, vc, v2, acc, length, omega*180/M_PI);
 257 */
 258         dbgPrintf("segment %p: vc1=%g\n", this, vc);
 259         acc = (v2-vc)*(v2+vc)/length;
 260             // acceleration needed
 261         acc = fmin(acc, constr.maxacc);
 262             // acceleration possible
 263
 264         v2 = sqrt(acc*length + pow(vc, 2));
 265         v1 = v2;
 266
 267         acc1 = -acc; // deccelerate
 268         acc2 = acc;  // accelerate
 269     }
 270
 271
 272     /**
 273      * converts distance to spline parameter
 274      */
 275     double Spline::dist2param(double distance) {
 276         double par;
 277         // FIXME: This cannot be so simple! Parameter is not linear to
 278         // distance.
 279         if (distance >= 0) {
 280             par = (param1-param0)*distance/length;
 281             par += param0;
 282         }
 283         else {
 284             distance = -distance;
 285             par = (param1-param0)*(length-distance)/length;
 286             par += param0;
 287         }
 288         return par;
 289     }
 290
 291
 292     /**
 293      * Returns the point of the segment, located at the specific @c
 294      * par (param0 - param1) from the beginning
 295      *
 296      * @param[in] par Parameter from 0 to 1 alont the segmet.
 297      * @param[out] p Pointer to the point to sore the result.
 298      */
 299     void Spline::getPointAtParam(double par, Point *p) {
 300         p->x = Ax*pow(par, 5) + Bx*pow(par, 4) + Cx*pow(par, 3) + Dx*pow(par, 2) + Ex*par + Fx;
 301         p->y = Ay*pow(par, 5) + By*pow(par, 4) + Cy*pow(par, 3) + Dy*pow(par, 2) + Ey*par + Fy;
 302     }
 303
 304
 305     void Spline::getPointAt(double distance, Point *p) {
 306         getPointAtParam(dist2param(distance), p);
 307     }
 308
 309
 310     /**
 311      * Calculates tangential direction of the robot
 312      */
 313     double Spline::getAngleAt(double distance) {
 314         double par;
 315         double x_, y_;
 316         double phi;
 317
 318         par = dist2param(distance);
 319         x_ = 5*Ax*pow(par, 4) + 4*Bx*pow(par, 3) + 3*Cx*pow(par, 2) + 2*Dx*par + Ex;
 320         y_ = 5*Ay*pow(par, 4) + 4*By*pow(par, 3) + 3*Cy*pow(par, 2) + 2*Dy*par + Ey;
 321
 322         phi = atan2(y_, x_);
 323         return phi;
 324     }
 325
 326     /**
 327      * counts recent radius of curvature
 328      * @return can be positive or negative, depending on left/right turning
 329      */
 330     double Spline::getRadiusAtParam(double par) {
 331         double x_ = 5*Ax*pow(par, 4) + 4*Bx*pow(par, 3) + 3*Cx*pow(par, 2) + 2*Dx*par + Ex;
 332         double y_ = 5*Ay*pow(par, 4) + 4*By*pow(par, 3) + 3*Cy*pow(par, 2) + 2*Dy*par + Ey;
 333         double x__ = 20*Ax*pow(par, 3) + 12*Bx*pow(par, 2) + 6*Cx*par + 2*Dy;
 334         double y__ = 20*Ay*pow(par, 3) + 12*By*pow(par, 2) + 6*Cy*par + 2*Dy;
 335
 336         return  sqrt( pow(pow(x_ ,2) + pow(y_ ,2) ,3) ) /
 337                 (x_*y__ - y_*x__);
 338             // by definition
 339     }
 340
 341     /**
 342      * counts recent radius of curvature
 343      * @return can be positive or negative, depending on left/right turning
 344      */
 345     double Spline::getRadiusAt(double distance) {
 346         return getRadiusAtParam(dist2param(distance));
 347     }
 348
 349
 350     /**
 351      * result is changed variable @c length and one of @c param0 and @c param1
 352      * changed is also @c p1 or @c p2, but they are no more utilized
 353      */
 354     void Spline::shortenBy(double distance, Point *newEnd) {
 355         getPointAt(-distance, newEnd);
 356         if (distance > 0) {     // end being cut off
 357             p2 = newEnd;
 358             param1 -= dist2param(distance);
 359
 360         } else {                // begining being cut off
 361             p1 = newEnd;
 362             distance = -distance;
 363             param0 += dist2param(distance);
 364         }
 365         length -= distance;
 366     }
 367
 368     TrajectorySegment* Spline::splitAt(double distance, Point *newEnd) {
 369         if (distance <= 0 || distance >= length)
 370             return NULL;
 371
 372         getPointAt(distance, newEnd);
 373
 374         Spline *ns = new Spline(*this);
 375         ns->shortenBy(-(length-distance), newEnd);
 376
 377         this->shortenBy(distance, newEnd);
 378
 379         return ns;
 380     }
 381
 382     TrajectorySegment* Spline::splitAtByTime(double time, Point *newEnd) {
 383         if (time <= t1 || time >= t2)
 384             return NULL;
 385         //getPointAt(getDistance(time), newEnd);
 386         double dst = getDistance(time);
 387
 388         Spline *ns = new Spline(*this);
 389         ns->shortenBy(-(dst), newEnd);
 390         this->shortenBy(length-dst, newEnd);
 391
 392         // need to update speeds
 393         if (time < tc) {
 394                 ns->v1 = v1 + acc1 * (time-t1);
 395                 v2     = v1 + acc1 * (time-t1);
 396         } else {
 397                 ns->v1 = vc + acc2 * (time-tc);
 398                 v2     = vc + acc2 * (time-tc);
 399         }
 400 //      printf("splitA %lf len %lf speeds v1 %lf v2 %lf, param %lf %lf\n", time, length,  v1, v2, param0, param1);
 401 //      printf("splitB %lf len %lf speeds v1 %lf v2 %lf\n", time, ns->length, ns->v1, ns->v2);
 402
 403         return ns;
 404     }
 405
 406     void Spline::getRefPos(double time, Pos &rp) {
 407         double radius;
 408         double distance = getDistance(time);
 409         double speed;
 410
 411         if (time < tc) {
 412             // deccelerating part starts in the beginning
 413             double t = time-t1;
 414             speed = v1 + acc1*t;
 415         }
 416         else {
 417             // accelerating part starts in the middle
 418             double t = time-tc;
 419             speed = vc + acc2*t;
 420         }
 421
 422
 423         Point * pt = new Point();
 424         getPointAt(distance, pt); // FIXME: Avoid this time consuming calculations
 425 //        getPointAtParam(param0+(param1-param0)*fraction, pt);
 426         rp.x = pt->x;
 427         rp.y = pt->y;
 428         rp.v = speed;
 429
 430         rp.phi = getAngleAt(distance);
 431         radius = getRadiusAt(distance);
 432         rp.omega =rp.v/radius;  // radius is either positive or negative
 433     }
 434
 435     double Spline::startAt(double time) {
 436         if (fabs(param0-0.0) < 0.0001 && fabs(param1-1.0) < 0.0001) {  // compute only the first time
 437                 acc2 = (v2-vc)*(v2+vc)/length;
 438                 acc1 = (vc-v1)*(vc+v1)/length;
 439         }
 440         // acceleration update after v1 and v2 changed (due to neighbour segments)
 441
 442         t1 = time;
 443         // FIXME: not so simple
 444
 445         // calculating passage duration from average speed
 446
 447         // decclelerating part duration
 448         double dpar = fmin(param1, 0.5) - fmin(param0, 0.5);
 449         double ds = dpar * (length / (param1-param0)); // passage distance in this part
 450         double vp = (v1+vc) / 2.0;        // average speed
 451         tc = t1 + ds/vp;
 452
 453         // plus acclelerating part duration
 454         dpar = fmax(param1, 0.5) - fmax(param0, 0.5);
 455         ds = dpar * (length / (param1-param0));
 456         vp = (vc+v2) / 2.0;
 457         t2 = tc + ds/vp;
 458
 459         return t2;
 460     }
 461
 462     /**
 463      * return distance on the spline in a given time
 464      */
 465     double Spline::getDistance(double time) const {
 466         if (time<tc) {
 467                 time -= t1;
 468                 return 0.5*acc1*time*time + v1*time;
 469         } else {
 470                 time -= tc;
 471                 return 0.5*acc2*time*time + vc*time + (0.5*length/(param1-param0));
 472         }
 473     }
 474
 475
 476 #ifdef MATLAB_MEX_FILE
 477     void Spline::plot(const char *style) {
 478         char cmd[300];
 479         const int len = 20;
 480         int i;
 481         Point *rp = new Point();
 482         mxArray *x = mxCreateDoubleMatrix(1, len, mxREAL);
 483         mxArray *y = mxCreateDoubleMatrix(1, len, mxREAL);
 484         mxArray *s = mxCreateCharMatrixFromStrings(1, &style);
 485         mxArray *rhs[] = {x,y,s};
 486
 487         for (i=0; i < len; i++) {
 488             getPointAtParam(param0+(param1-param0)*i/(len), rp);
 489             mxGetPr(x)[i] = rp->x;
 490             mxGetPr(y)[i] = rp->y;
 491         }
 492         mexCallMATLAB(0, NULL, 3, rhs, "plot");
 493         sprintf(cmd, "plot([%g %g], [%g %g], 'mo')",
 494                 p1->x, p2->x, p1->y, p2->y);
 495         mexEvalString(cmd);
 496         mxDestroyArray(x);
 497         mxDestroyArray(y);
 498         mxDestroyArray(s);
 499     };
 500
 501     void Spline::plotSpeed(const char *style) {
 502         char cmd[300];
 503         if (param0 <= 0.5) {
 504             sprintf(cmd, "plot([%g %g], [%g %g], '%s')",
 505                 t1, tc, v1, vc, style);
 506             mexEvalString(cmd);
 507         }
 508         if (param1 >= 0.5) {
 509             sprintf(cmd, "plot([%g %g], [%g %g], '%s')",
 510                 tc, t2, vc, v2, style);
 511             mexEvalString(cmd);
 512         }
 513     }
 514 #endif
 515
 516 } // namespace Segment