src/motion/trgen.cc

   1 /**
   2  * @file   trgen.cc
   3  * @author Michal Sojka
   4  * @date   Sun Aug 12 09:59:32 2007
   5  *
   6  * @brief  Trajectory Generator
   7  *
   8  *
   9  */
  10
  11 #include <exception>
  12 #include <typeinfo>
  13 #include <iterator>
  14 #include "trgen.h"
  15
  16 //#define DEBUG
  17
  18 #ifdef DEBUG
  19   #ifdef MATLAB_MEX_FILE
  20     #define dbgPrintf(format, ...) ssPrintf(format, ##__VA_ARGS__)
  21   #else
  22     #define dbgPrintf(format, ...) printf(format, ##__VA_ARGS__)
  23   #endif
  24 #else
  25 #define dbgPrintf(format, ...)
  26 #endif
  27
  28 #ifdef MATLAB_MEX_FILE
  29 #define S_FUNCTION_LEVEL 2
  30 #define S_FUNCTION_NAME  sf_trgen
  31 #include "simstruc.h"
  32 #endif
  33
  34 namespace Segment {
  35
  36 /**
  37  * Stright segment of the trajectory.
  38  */
  39 class Line : public TrajectorySegment
  40 {
  41     Point *p1, *p2;
  42     double angle;               // uhle v globalnich souradnicich
  43     double length;              // delka segmentu
  44     double cosphi, sinphi;
  45 public:
  46     Line(Point *_p1, Point *_p2) : p1(_p1), p2(_p2) {
  47         length = p1->distanceTo(*p2);
  48         angle = p1->angleTo(*p2);
  49         sinphi = (p2->y-p1->y)/length;
  50         cosphi = (p2->x-p1->x)/length;
  51     }
  52     virtual ~Line() {};
  53
  54     virtual void setMaxV(const TrajectoryConstraints &constr) {
  55         maxv = constr.maxv;
  56     }
  57
  58     virtual double getLength() const {return length;}
  59     double getAngle() {return angle;}
  60
  61     virtual void getPointAt(double distance, Point *p) {
  62         double ratio;
  63         if (distance > 0) {
  64             ratio = distance/length;
  65         } else {
  66             ratio = (length+distance)/length;
  67         }
  68         p->x = p1->x + ratio*(p2->x - p1->x);
  69         p->y = p1->y + ratio*(p2->y - p1->y);
  70     }
  71
  72     virtual void shortenBy(double distance, Point *newEnd) {
  73         getPointAt(-distance, newEnd);
  74         if (distance > 0) {
  75             p2 = newEnd;
  76         } else {
  77             distance = -distance;
  78             p1 = newEnd;
  79         }
  80         length -= distance;
  81     }
  82
  83     virtual TrajectorySegment* splitAt(double distance, Point *newEnd) {
  84         if (distance <= 0 || distance >= length) {
  85             dbgPrintf("splitAt: distance=%g length=%g\n", distance, length);
  86             return NULL;
  87         }
  88
  89         getPointAt(distance, newEnd);
  90         Line *ns = new Line(*this);
  91         p2 = newEnd;
  92         length = distance;
  93         ns->length -= distance;
  94         ns->p1 = newEnd;
  95         return ns;
  96     }
  97
  98     virtual void getRefPos(double time, Pos &rp) {
  99         double t = time-t1;
 100         double fraction = t/(t2-t1);
 101         double distance = (v1 + 0.5*acc*t) * t;
 102         rp.x = p1->x + distance*cosphi;
 103         rp.y = p1->y + distance*sinphi;
 104         rp.phi = angle;
 105
 106         rp.v = v1+fraction*(v2-v1);
 107         rp.omega = 0;
 108
 109     }
 110 #ifdef MATLAB_MEX_FILE
 111     virtual void plot(const char *style) {
 112         char cmd[300];
 113         sprintf(cmd, "plot([%g %g], [%g %g], '%so')",
 114                 p1->x, p2->x, p1->y, p2->y, style);
 115         mexEvalString(cmd);
 116     };
 117 #endif
 118 };
 119
 120 /**
 121  * Arc shaped segment.
 122  *
 123  */
 124 class Arc : public TrajectorySegment
 125 {
 126     Point *p1, *p2;
 127     double angle;               // vrcholovy uhel [-pi,+pi]
 128     double startAngle;          // uhel od stredu k p1
 129     double radius;              // polomer
 130     double length;              // delka segmentu
 131     Point center;
 132 public:
 133     Arc(Point *_p1, Point *_p2, double _radius) :
 134             p1(_p1), p2(_p2), radius(fabs(_radius)) {
 135
 136         // Find the center of the arc
 137         double angp1p2 = p1->angleTo(*p2);
 138         double m = p1->distanceTo(*p2)/2.0;
 139         if (radius < m) {
 140             radius = m;
 141             dbgPrintf("EEEERRRRRRRRROOOOOOOOORRRRRRRRR!!!!!!!!!!\n");
 142         }
 143         double angcen = acos(m/radius);
 144         if (_radius < 0) angcen = -angcen;
 145         center = Point(p1->x + radius * cos(angp1p2+angcen),
 146                        p1->y + radius * sin(angp1p2+angcen));
 147
 148         startAngle = center.angleTo(*p1);
 149         angle = center.angleTo(*p2) - startAngle;
 150         if (angle < -M_PI) angle += 2.0*M_PI;
 151         if (angle > +M_PI) angle -= 2.0*M_PI;
 152
 153         length = fabs(angle*radius);
 154     }
 155     virtual ~Arc() {};
 156
 157
 158     virtual void setMaxV(const TrajectoryConstraints &constr) {
 159         double r = radius;
 160         maxv = fmin(constr.maxv, constr.maxomega * r);
 161         maxv = fmin(maxv, sqrt(constr.maxcenacc * r));
 162     }
 163
 164     virtual double getLength() const {return length;}
 165
 166     virtual void getPointAt(double distance, Point *p) {
 167         double ratio, a;
 168         if (distance > 0) {
 169             ratio = distance/length;
 170         } else {
 171             distance = -distance;
 172             ratio = (length-distance)/length;
 173         }
 174         a = startAngle + ratio*angle;
 175         p->x = center.x + radius*cos(a);
 176         p->y = center.y + radius*sin(a);
 177     }
 178     virtual void shortenBy(double distance, Point *newEnd) {
 179         getPointAt(-distance, newEnd);
 180         if (distance > 0) {
 181             angle *= (length-distance)/length;
 182             p2 = newEnd;
 183         } else {
 184             distance = -distance;
 185             startAngle = startAngle + distance/length*angle;
 186             angle *= (length-distance)/length;
 187             p1 = newEnd;
 188         }
 189         length -= distance;
 190     }
 191
 192     virtual TrajectorySegment* splitAt(double distance, Point *newEnd) {
 193         if (distance <= 0 || distance >= length)
 194             return NULL;
 195
 196         getPointAt(distance, newEnd);
 197         Arc *ns = new Arc(*this);
 198
 199         double a = distance/length*angle;
 200         angle = a;
 201         length = distance;
 202         p2 = newEnd;
 203
 204         ns->startAngle += a;
 205         ns->angle -= a;
 206         ns->length -= distance;
 207         ns->p1 = newEnd;
 208         return ns;
 209     }
 210
 211     virtual void getRefPos(double time, Pos &rp) {
 212         double t = time-t1;
 213         double fraction = t/(t2-t1);
 214         double distance = (v1 + 0.5*acc*t) * t;
 215         double a = startAngle + distance/length*angle;
 216         rp.x = center.x + radius*cos(a);
 217         rp.y = center.y + radius*sin(a);
 218         if (angle > 0)
 219             rp.phi = a + M_PI/2.0;
 220         else
 221             rp.phi = a - M_PI/2.0;
 222
 223         rp.v = v1+fraction*(v2-v1);
 224         rp.omega =rp.v/radius;
 225         if (angle < 0) rp.omega = -rp.omega;
 226
 227     }
 228 #ifdef MATLAB_MEX_FILE
 229     virtual void plot(const char *style) {
 230         char cmd[300];
 231         const int len = 10;
 232         int i;
 233         Pos rp;
 234         mxArray *x = mxCreateDoubleMatrix(1, len, mxREAL);
 235         mxArray *y = mxCreateDoubleMatrix(1, len, mxREAL);
 236         mxArray *s = mxCreateCharMatrixFromStrings(1, &style);
 237         mxArray *rhs[] = {x,y,s};
 238
 239         for (i=0; i < len; i++) {
 240             getRefPos(t1+(t2-t1)*i/(len-1), rp);
 241             mxGetPr(x)[i] = rp.x;
 242             mxGetPr(y)[i] = rp.y;
 243         }
 244         mexCallMATLAB(0, NULL, 3, rhs, "plot");
 245         sprintf(cmd, "plot([%g %g], [%g %g], 'mo')",
 246                 p1->x, p2->x, p1->y, p2->y);
 247         mexEvalString(cmd);
 248         mxDestroyArray(x);
 249         mxDestroyArray(y);
 250         mxDestroyArray(s);
 251     };
 252 #endif
 253 };
 254
 255 /**
 256  * Segment representing turn at one place.
 257  *
 258  * Speed related variables @c v1, @c v2 and @c acc represents
 259  * angular speed here.
 260  *
 261  */
 262 class Turn : public TrajectorySegment
 263 {
 264     Point *center;
 265     double turnBy;            /// turn by this angle [rad]
 266     double startHeading;      /// turning starts at this heading [rad]
 267 public:
 268     Turn(Point *_center, double _startHeading, double _turnBy) :
 269         center(_center), turnBy(_turnBy), startHeading(_startHeading) {}
 270     virtual ~Turn() {};
 271
 272
 273     virtual void setMaxV(const TrajectoryConstraints &constr) {
 274             maxv = constr.maxomega;
 275     }
 276
 277     virtual double getMaxAcc(const TrajectoryConstraints &constr) const {
 278         return constr.maxangacc;
 279     }
 280
 281     virtual bool isTurn() const { return true; };
 282
 283     virtual double getLength() const {return 0;}
 284     virtual double getUnifiedLength() const {
 285         return fabs(turnBy);
 286     }
 287
 288     virtual void getPointAt(double distance, Point *p) {
 289         *p = *center;
 290     }
 291     virtual void shortenBy(double distance, Point *newEnd) {}
 292
 293     virtual TrajectorySegment* splitAt(double distance, Point *newEnd) {
 294         if (distance <= 0 || distance >= fabs(turnBy)) {
 295             dbgPrintf("splitAt: distance=%g turnBy=%g\n", distance, turnBy);
 296             return NULL;
 297         }
 298
 299         Turn *ns = new Turn(*this);
 300         if (turnBy < 0)
 301                 distance = -distance;
 302         turnBy = distance;
 303         ns->startHeading+=distance;
 304         ns->turnBy -= distance;
 305         return ns;
 306     }
 307
 308     virtual void getRefPos(double time, Pos &rp) {
 309         double t = time-t1;
 310         double fraction = t/(t2-t1);
 311         double distance = (v1 + 0.5*acc*t) * t;
 312         rp.x = center->x;
 313         rp.y = center->y;
 314         if (turnBy > 0)
 315             rp.phi = startHeading + distance;
 316         else
 317             rp.phi = startHeading - distance;
 318         rp.v=0;
 319         if (time < t2) {
 320                 rp.omega = v1+fraction*(v2-v1);
 321                 if (turnBy < 0) rp.omega = -rp.omega;
 322         }
 323         else
 324                 rp.omega = 0;
 325     }
 326
 327 #ifdef MATLAB_MEX_FILE
 328     virtual void plot(const char *style) {};
 329 #endif
 330 };
 331
 332 } // namespace Segment
 333
 334
 335 using namespace Segment;
 336
 337 /**
 338  * Split a sergment and add the newly created segment to the
 339  * trajectory.
 340  *
 341  * @param seg Segment to split.
 342  * @param distance Where to split.
 343  *
 344  * @return
 345  */
 346 bool
 347 Trajectory::splitSegment(iterator &seg, double distance)
 348 {
 349     TrajectorySegment *ns;
 350     Point *np = NULL;
 351     if (typeid(**seg) != typeid(Turn))
 352         np = new Point;
 353
 354     ns = (*seg)->splitAt(distance, np);
 355     if (ns == NULL) {
 356         dbgPrintf("ERROR\n");
 357         delete(np);
 358         return false;
 359     }
 360     if (np)
 361         wayPoints.push_back(np);
 362     seg = insert(++seg, ns);
 363     return true;
 364 }
 365
 366 //     iterator insert(reverse_iterator it, const value_type& val) {
 367 //         return insert((--it).base(), val);
 368 //     }
 369
 370 /**
 371  * Connects points by line segments.
 372  */
 373 bool
 374 Trajectory::points2segments()
 375 {
 376     TrajectoryPoints::iterator p1, p2;
 377     Point ip(initPos.x, initPos.y);
 378
 379     if (*wayPoints.front() != ip) {
 380         initialPoint = new Point(ip);
 381         wayPoints.push_front(initialPoint);
 382     } else
 383         initialPoint = wayPoints.front();
 384
 385     // Delete old segments
 386     deleteSegments();
 387
 388     if (wayPoints.size() < 2 &&
 389         (finalHeading.turn_type == FH_DONT_TURN ||
 390          (finalHeading.turn_type == FH_SHORTEST && \
 391           fabs(initPos.phi - finalHeading.heading) < 1.0/180*M_PI))) {
 392         return false;
 393     }
 394     for (p1 = wayPoints.begin(), p2 = ++wayPoints.begin();
 395          p2 != wayPoints.end();
 396          p1++, p2++) {
 397         push_back(new Line(*p1, *p2));
 398     }
 399     return true;
 400 }
 401
 402
 403 /**
 404  * Converts corners in trajectory to arcs.
 405  * @todo Splines will be better.
 406  */
 407
 408 // \f{center}
 409 // \frame{\begin{tikzpicture}
 410 //  \draw (-5cm,0) coordinate(p1) node[left]{$p_1$} --
 411 //   node[sloped,above]{$\mathrm seg_1$} (0,0) coordinate(p2)
 412 //   node[above]{$p_2$} -- node[sloped,above]{$\mathrm seg_2$} +(-30:5cm)
 413 //   node[right]{$p_3$} coordinate (p3);
 414 //   \coordinate[shift=(-105:5cm)] (c) at (p2);
 415 //   \draw (p2) -- (c);
 416 //   \draw (c) -- (c |- p2);
 417 //   \draw (c) -- (intersection of c--40:10cm and p2--p3);
 418 // \end{tikzpicture}}
 419 //    \f}
 420
 421 void
 422 Trajectory::corners2arcs()
 423 {
 424     iterator seg, segNext, segPrev;
 425
 426     if (size() < 2) return;
 427
 428     // Find radiuses to meet e <= maxe
 429     for (segPrev = begin(), segNext = ++begin();
 430          segNext != end();
 431          segPrev++, segNext++) {
 432
 433         double alpha;       // curve angle (-pi <= alpha <= +pi)
 434         double r;           // radius of proposed arc. (alpha < 0 -> r < 0)
 435         double s;           // length of the segment replaced by the arc (s > 0)
 436         try {
 437             alpha = dynamic_cast<Line*>(*segNext)->getAngle() -
 438                 dynamic_cast<Line*>(*segPrev)->getAngle();
 439             if (alpha > +M_PI) alpha -= 2.0*M_PI;
 440             if (alpha < -M_PI) alpha += 2.0*M_PI;
 441
 442             if (fabs(alpha) < M_PI/180.0) continue;
 443
 444             r = constr.maxe / (1/cos(alpha/2.0) - 1);
 445             if (alpha < 0) r = -r;
 446             s = r*tan(alpha/2.0);
 447             if (s > (*segPrev)->getLength()) {
 448                 s = (*segPrev)->getLength();
 449                 r = s/tan(alpha/2.0);
 450             }
 451             if (s > (*segNext)->getLength()) {
 452                 s = (*segNext)->getLength();
 453                 r = s/tan(alpha/2.0);
 454             }
 455             (*segPrev)->r = r;
 456             (*segPrev)->s = s;
 457             (*segPrev)->alphahalf = alpha/2.0;
 458         }
 459         catch (std::exception& e) {} // Skip wrong dynamic_cast
 460     }
 461
 462     if (size() >= 3) {
 463         // Reduce overlapping arcs by decreasing radiuses
 464         double s1, s2, r1, r2, l, alphahalf1, alphahalf2;
 465         bool wantSameR;
 466         for (segPrev = begin(), segNext = ++begin();
 467              segNext != --end();
 468              segPrev++, segNext++) {
 469             l = (*segNext)->getLength();
 470             s1 = (*segPrev)->s;
 471             s2 = (*segNext)->s;
 472             r1 = (*segPrev)->r;
 473             r2 = (*segNext)->r;
 474             alphahalf1 = (*segPrev)->alphahalf;
 475             alphahalf2 = (*segNext)->alphahalf;
 476             if (s1 + s2 > l) {
 477                 if (fabs(r1) > fabs(r2)) {
 478                     s1 = l - s2;
 479                     r1 = s1/tan(alphahalf1);
 480                     wantSameR = (fabs(r1) < fabs(r2));
 481                 } else {
 482                     s2 = l - s1;
 483                     r2 = s2/tan(alphahalf2);
 484                     wantSameR = (fabs(r1) > fabs(r2));
 485                 }
 486                 if (wantSameR) {
 487                     if (alphahalf1 < 0 ^ alphahalf2 < 0)
 488                         s1 = l*tan(alphahalf1)/(tan(alphahalf1)-tan(alphahalf2));
 489                     else
 490                         s1 = l*tan(alphahalf1)/(tan(alphahalf1)+tan(alphahalf2));
 491                     s2 = l - s1;
 492                     r1 = s1/tan(alphahalf1);
 493                     r2 = s2/tan(alphahalf2);
 494                 }
 495                 (*segPrev)->s = s1;
 496                 (*segNext)->s = s2;
 497                 (*segPrev)->r = r1;
 498                 (*segNext)->r = r2;
 499             }
 500         }
 501     }
 502
 503     // replace corners by arcs with previously computed radiuses
 504     for (segPrev = begin(), segNext = ++begin();
 505          segNext != end();
 506          ++segPrev, ++segNext) {
 507         if (fabs((*segPrev)->r) > 0) {
 508             Point *newEnd1, *newEnd2;
 509             double s = (*segPrev)->s;
 510             newEnd1 = new Point();
 511             newEnd2 = new Point();
 512             (*segPrev)->shortenBy(+s, newEnd1);
 513             (*segNext)->shortenBy(-s, newEnd2);
 514             wayPoints.push_back(newEnd1);
 515             wayPoints.push_back(newEnd2);
 516             segPrev = insert(segNext, new Arc(newEnd1, newEnd2,
 517                                                         (*segPrev)->r));
 518         }
 519     }
 520
 521     //delete zero-length segments
 522     seg = begin();
 523     while (seg != end()) {
 524         if ((*seg)->getLength() <= 1e-9) {
 525             delete(*seg);
 526             seg = erase(seg);
 527         } else
 528             seg++;
 529     }
 530 }
 531
 532
 533 /**
 534  * Assigns speeds and accelerations to segments; tries to maximize
 535  * speed where possible. If necessary it also splits long segments
 536  * to multiple parts with maximum acceleration/deceleration and
 537  * maximal speed. The optimization is done in three passes. In the
 538  * first pass, maximal speed of segments are set up (see
 539  * TrajectoryConstraints::maxv, TrajectoryConstraints::maxomega,
 540  * TrajectoryConstraints::maxcenacc). Then follows a forward pass, where
 541  * acceleration is added to join segments with increasing maximal
 542  * speeds. Finally, backward pass does almost the same by adding
 543  * deceleration segments.
 544  *
 545  * Finding the split point of the segment where deceleration
 546  * immediately follows acceleration is based on the following
 547  * calculations:
 548  *
 549  * For uniformly accelerated motion, we can calculate time as
 550  * function of speed: \f[ t(v)=\frac{v-v_0}{a}\f] Using this, we can
 551  * express distance \f$ s \f$ as a function of speed: \f[ s(v) =
 552  * \frac{1}{2} at(v)^2 + v_0 t(v) = \frac{v^2 - v_0^2}{2a}\f]
 553  *
 554  * Then, distance traveled during acceleration (\f$ s_1 \f$) and
 555  * deceleration (\f$ s_2 \f$) must be equal to the length of the
 556  * segment \f$ l = s_1 + s_2 \f$. Considering a segment with initial
 557  * speed \f$ v_1 \f$, final speed \f$ v_2 \f$, and maximal speed (in
 558  * the split point) \f$ v_{max} \f$, we can derive: \f[ l =
 559  * s_1(v_{max}) + s_2(v_{max}) = \frac{2v_{max}^2 - v_1^2 - v_2^2}{2a}
 560  * \f] \f[ v_{max}^2 = al + \frac{v_1^2 + v_2^2}{2} \f]
 561  *
 562  * Then, the length of the segment before split is: \f[ s_1 = l - s_2
 563  * = l - \frac{v_{max}^2-v_2^2}{2a} = \frac{1}{2}l + \frac{v_2^2 -
 564  * v_1^2}{4a} \f]
 565  *
 566  */
 567 void
 568 Trajectory::calcSpeeds()
 569 {
 570     if (size() == 0) return;
 571
 572     // Assign maximum speed
 573     for (iterator seg = begin(); seg != end(); seg++) {
 574         (*seg)->setMaxV(constr);
 575         (*seg)->v1 = (*seg)->maxv;
 576         (*seg)->v2 = (*seg)->maxv;
 577     }
 578 #ifdef MATLAB_MEX_FILE
 579     plotSpeed("r-o");
 580 #endif
 581
 582     double lastv;
 583     bool turning = false;       // FIXME: turning je integer podle smeru otaceni
 584
 585     // Accelerare where possible
 586     lastv = initPos.v;
 587     for (iterator seg = begin(); seg != end(); seg++) {
 588         if (turning != (*seg)->isTurn()) {
 589             turning = (*seg)->isTurn();
 590             lastv = 0;
 591         }
 592         double v1 = (*seg)->v1;
 593         double v2 = (*seg)->v2;
 594         double maxv = (*seg)->maxv;
 595         //dbgPrintf("processing: %p v1=%g\n", *seg, v1);
 596         if (v1 > lastv) {
 597             v1 = lastv;
 598             double l = (*seg)->getUnifiedLength();
 599             double a = (*seg)->getMaxAcc(constr);
 600             double t;
 601
 602             t = (-v1+sqrt(v1*v1 + 2.0*a*l))/a;
 603             v2 = v1+a*t;
 604
 605             //dbgPrintf("t=%g v2=%g l=%g\n", t, v2, l);
 606             if (v2 > maxv) {    // split this segment
 607                 v2 = maxv;
 608                 t = (v2 - v1)/a;
 609                 l = v1*t+0.5*a*t*t;
 610                 //dbgPrintf("t=%g v2=%g l=%g\n", t, v2, l);
 611                 iterator ns = seg;
 612                 if (splitSegment(ns, l)) {
 613                     (*ns)->v1 = v2;
 614                 }
 615             }
 616
 617             (*seg)->v1 = v1;
 618             (*seg)->v2 = v2;
 619         }
 620         lastv = (*seg)->v2;
 621     }
 622 #ifdef MATLAB_MEX_FILE
 623     plotSpeed("g-o");
 624 #endif
 625
 626     // Deccelerate where needed
 627     turning = false;
 628     lastv = 0;              // Final speed
 629     for (reverse_iterator seg = rbegin(); seg != rend(); seg++) {
 630         if (turning != (*seg)->isTurn()) {
 631             turning = (*seg)->isTurn();
 632             lastv = 0;
 633         }
 634         double v1 = (*seg)->v1;
 635         double v2 = (*seg)->v2;
 636         double maxv = (*seg)->maxv;
 637         dbgPrintf("processing: %p v2=%g\n", *seg, v2);
 638         if (v2 > lastv) {
 639             v2 = lastv;
 640             double l = (*seg)->getUnifiedLength();
 641             double a = (*seg)->getMaxAcc(constr);
 642             double t;
 643
 644             t = (-v2+sqrt(v2*v2 + 2.0*a*l))/a;
 645             v1 = v2+a*t;
 646
 647             //dbgPrintf("t=%g v1=%g v2=%g l=%g\n", t, v1, v2, l);
 648             if (v1 > maxv && (*seg)->v1 == maxv) {
 649                 v1 = maxv;
 650                 t = (v1 - v2)/a;
 651                 double l2 = v2*t+0.5*a*t*t;
 652                 //dbgPrintf("t=%g v1=%g v2=%g l=%g l2=%g\n", t, v1, v2, l, l2);
 653                 iterator ns = --(seg.base());
 654                 //dbgPrintf("seg=%p ns=%p\n", *seg, *ns);
 655                 if (splitSegment(ns, l-l2)) {
 656                     //dbgPrintf("ns=%p\n", *ns);
 657                     (*ns)->v2 = v1;
 658                     (*seg)->v1 = v1;
 659                 }
 660             } else if (v1 > (*seg)->v1) {
 661                 // Acceleration and deacceleration within one segment
 662                 // Where is the crossing af acceleration and decceleration?
 663                 v1 = (*seg)->v1;
 664                 double s1 = l/2.0 + (v2*v2 - v1*v1)/4.0*a;
 665
 666                 if (s1 > l/1000 && s1 < l - l/1000) {
 667                     //dbgPrintf("t=%g v1=%g v2=%g l=%g s1=%g\n", t, v1, v2, l, s1);
 668                     iterator ns = --(seg.base());
 669                     //dbgPrintf("seg=%p ns=%p (before split)\n", *seg, *ns);
 670                     if (splitSegment(ns, s1)) {
 671                         ++seg; //move to the earlier subsegment of the splited one
 672                         //dbgPrintf("seg=%p ns=%p (after split)\n", *seg, *ns);
 673                         v1 = sqrt(v1*v1 + 2.0*a*s1);
 674                         (*ns)->v1 = v1;
 675                         (*ns)->v2 = v2;
 676                         v2 = v1;
 677                         v1 = (*seg)->v1;
 678                     }
 679                 }
 680             } else {
 681                 (*seg)->v1 = v1;
 682             }
 683             (*seg)->v2 = v2;
 684         }
 685         lastv = (*seg)->v1;
 686     }
 687 #ifdef MATLAB_MEX_FILE
 688     plotSpeed("b-o");
 689 #endif
 690     for (iterator seg = begin(); seg != end(); ++seg) {
 691         dbgPrintf("final: %-16s %p v1=%-8.2g v2=%-8.2g l=%-8.2g\n",
 692                   typeid(**seg).name(),
 693                   *seg, (*seg)->v1, (*seg)->v2, (*seg)->getLength());
 694     }
 695 }
 696
 697
 698 double
 699 Trajectory::calcLength()
 700 {
 701     double totLen = 0;
 702     for (iterator seg = begin(); seg != end(); ++seg) {
 703         totLen += (*seg)->getLength();
 704     }
 705     return totLen;
 706 }
 707
 708 static double minimizeAngle(double rad)
 709 {
 710     rad = fmod(rad, 2.0*M_PI);
 711     if (rad > +M_PI) rad -= 2.0*M_PI;
 712     if (rad < -M_PI) rad += 2.0*M_PI;
 713     return rad;
 714 }
 715
 716 static double calcTurnBy(double initialHeading, struct final_heading finalHeading)
 717 {
 718         double turnBy;
 719         if (finalHeading.turn_type == FH_DONT_TURN) turnBy = NAN;
 720         else {
 721                 turnBy = minimizeAngle(finalHeading.heading - initialHeading);
 722                 if (turnBy < 0 && finalHeading.turn_type == FH_CCW) turnBy += M_PI*2.0;
 723                 if (turnBy > 0 && finalHeading.turn_type == FH_CW) turnBy -= M_PI*2.0;
 724         }
 725         return turnBy;
 726 }
 727
 728
 729 void Trajectory::addTurns()
 730 {
 731     Pos p;
 732     TrajectorySegment *seg;
 733     double initialHeading = initPos.phi;
 734     double turnBy;
 735     if (backward)
 736         initialHeading -= M_PI;
 737
 738     if (size() == 0) {
 739         // We have to turn only. No movement.
 740         dbgPrintf("*** Adding only the turn - start: %5.2f, end %5.2f\n", initialHeading/M_PI*180, finalHeading/M_PI*180);
 741         turnBy = calcTurnBy(initialHeading, finalHeading);
 742         if (fabs(turnBy) > 1e-3) {
 743             push_front(new Turn(initialPoint, initialHeading, turnBy));
 744         }
 745     } else {
 746         // Add turn at beginning
 747         seg = *begin();
 748         seg->v1 = 1;           // Hack - speeds are not calculated yet
 749         seg->v2 = 1;
 750         seg->startAt(0);        // Assign times so we can use
 751                                 // getRefPos();
 752         seg->getRefPos(0, p);
 753         dbgPrintf("*** Adding initial turn - start: %5.2f, end %5.2f\n", initialHeading/M_PI*180, p.phi/M_PI*180);
 754         turnBy = minimizeAngle(p.phi - initialHeading);
 755         if (fabs(turnBy) > 1e-3) {
 756             push_front(new Turn(initialPoint, initialHeading, turnBy));
 757         }
 758
 759         // Add turn at the end
 760         seg = *(--end());
 761         seg->v1 = 1;           // Hack - speeds are not calculated yet
 762         seg->v2 = 1;
 763         seg->startAt(0);            // Assign times so we can use getRefPos();
 764         seg->getRefPos(seg->getT2(), p);
 765         turnBy = calcTurnBy(p.phi, finalHeading);
 766         if (fabs(turnBy) > 1e-3) { // NaN in finalHeading makes the condition false
 767             dbgPrintf("*** Adding final turn - start: %5.2f, end %5.2f\n", p.phi/M_PI*180, finalHeading/M_PI*180);
 768             push_back(new Turn(finalPoint, p.phi, turnBy));
 769         }
 770     }
 771 }
 772
 773 /**
 774  * Prepares the trajectory. It converts points added by addPoint() to
 775  * line segments (::Line). Then it replaces by sharp
 776  * corners by arcs (see corners2arcs()), then it adds turns at the
 777  * begin and end and finally it calculates speeds in the different
 778  * parts of the trajectory by adding acceleration and decceleration
 779  * segments (see calcSpeeds()).
 780  *
 781  * @param _initPos The point where the trajectory should
 782  * start. Typically the current robot position.
 783  * @todo Solve properly the case, when initPos has non-zero speed.
 784  * @return True in case of success, false when the trajectory has only
 785  * one point and initPos is already equal to that point.
 786  */
 787 bool
 788 Trajectory::prepare(Pos _initPos)
 789 {
 790     if (constr.maxv      <= 0 || constr.maxv      > 10 ||
 791         constr.maxomega  <= 0 || constr.maxomega  > 10 ||
 792         constr.maxangacc <= 0 || constr.maxangacc > 10 ||
 793         constr.maxacc    <= 0 || constr.maxacc    > 10 ||
 794         constr.maxcenacc <= 0 || constr.maxcenacc > 10 ||
 795         constr.maxe      <= 0 || constr.maxe      > 10) {
 796         dbgPrintf("wrong constraints!!!!\n");
 797     }
 798
 799     if (wayPoints.size() == 0) {
 800         dbgPrintf("No point in trajectory!\n");
 801         return false;
 802     }
 803
 804     initPos = _initPos;
 805     if (points2segments() == false) {
 806         dbgPrintf("points2segments error!\n");
 807         return false;
 808     }
 809     corners2arcs();
 810     addTurns();
 811     if (size() == 0)
 812         return false;
 813     calcSpeeds();
 814
 815     // Assign times to segments
 816     double t=0;
 817     for (iterator seg = begin(); seg != end(); ++seg) {
 818         t = (*seg)->startAt(t);
 819     }
 820     currentSeg = begin();
 821     prepared = true;
 822     return true;
 823 }
 824
 825 /**
 826  * Returns reference position of the trajectory at a given time.
 827  *
 828  * @param[in]  time Time from the beginning of the trajectory.
 829  * @param[out] rp  Returned reference position.
 830  * @return True if the time is after the trajectory ends or
 831  *         error. False otherwise.
 832  */
 833 bool
 834 Trajectory::getRefPos(double time, Pos &rp)
 835 {
 836     int d;
 837     bool ret = false;
 838     TrajectorySegment *seg;
 839
 840     if (!prepared) {
 841         ret = true;
 842     } else {
 843         do {
 844             d=(*currentSeg)->containsTime(time);
 845             if (d==0) break;
 846             if (currentSeg == --end() && d > 0) break;
 847             if (currentSeg == begin() && d < 0) break;
 848             if (d>0) ++currentSeg;
 849             else if (d<0) --currentSeg;
 850         } while (1);
 851
 852         seg = *currentSeg;
 853         if (d==0) {
 854             getSegRefPos(seg, time, rp);
 855         } else if (d<0) {
 856             getSegRefPos(seg, seg->getT1(), rp);
 857         } else {
 858             getSegRefPos(seg, seg->getT2(), rp);
 859             ret = true;             // We are at the end.
 860         }
 861     }
 862
 863     return ret;
 864 }
 865
 866 /**
 867  * Returns reference position of the given segment in the trajectory
 868  * and takes @c backward into the account.
 869  *
 870  * @param[in]  seg Segment to get the position from.
 871  * @param[in]  time Time from the beginning of the trajectory.
 872  * @param[out] rp  Returned reference position.
 873  */
 874 void
 875 Trajectory::getSegRefPos(TrajectorySegment *seg, double time, Pos &rp)
 876 {
 877     seg->getRefPos(time, rp);
 878
 879     if (backward) {
 880         rp.phi += M_PI;
 881         rp.v = -rp.v;
 882     }
 883 }
 884
 885 #ifdef MATLAB_MEX_FILE
 886 void
 887 Trajectory::plot(const char *style)
 888 {
 889     for (iterator seg = begin(); seg != end(); ++seg) {
 890         (*seg)->plot(style);
 891     }
 892 };
 893 void
 894 Trajectory::plotSpeed(const char *style)
 895 {
 896     const char *f2 = "figure(2); xlabel('Time [s]'); ylabel('Speed [m/s], dotted [rad/s]');";
 897     const char *f1 = "hold on; figure(1)";
 898     char turnstyle[20];
 899     strcpy(turnstyle, style);
 900     if (turnstyle[1] == '-') turnstyle[1] = ':';
 901     double t = 0;
 902     mexEvalString(f2);
 903     for (iterator seg = begin(); seg != end(); ++seg) {
 904         t = (*seg)->startAt(t);
 905         if (!(*seg)->isTurn())
 906             (*seg)->plotSpeed(style);
 907         else
 908             (*seg)->plotSpeed(turnstyle);
 909     }
 910     mexEvalString(f1);
 911 };
 912 #endif
 913