src/motion/trajectory.cc

   1 /**
   2  * @file   trajectory.cc
   3  * @author Michal Sojka, Petr Beneš
   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 #include "trgendbg.h"
  16 #include <string.h>
  17
  18 //#define MOTION_LOG
  19
  20 // #ifdef MATLAB_MEX_FILE
  21 // #define S_FUNCTION_LEVEL 2
  22 // #define S_FUNCTION_NAME  sf_trgen
  23 // #include "simstruc.h"
  24 // #endif
  25
  26 using namespace Segment;
  27
  28 /**
  29  * Split a sergment and add the newly created segment to the
  30  * trajectory.
  31  *
  32  * @param seg Segment to split.
  33  * @param time Where to split.
  34  *
  35  * @return
  36  */
  37 bool
  38 Trajectory::splitSegmentByTime(iterator &seg, double time)
  39 {
  40     TrajectorySegment *ns;
  41     Point *np = NULL;
  42     if (typeid(**seg) != typeid(Turn))
  43         np = new Point;
  44
  45     ns = (*seg)->splitAtByTime(time, np);
  46     if (ns == NULL) {
  47         dbgPrintf("ERROR\n");
  48         delete(np);
  49         return false;
  50     }
  51     if (np)
  52         wayPoints.push_back(np);
  53     seg = insert(++seg, ns);
  54     return true;
  55 }
  56
  57
  58 /**
  59  * Split a sergment and add the newly created segment to the
  60  * trajectory.
  61  *
  62  * @param seg Segment to split.
  63  * @param distance Where to split.
  64  *
  65  * @return
  66  */
  67 bool
  68 Trajectory::splitSegment(iterator &seg, double distance)
  69 {
  70     TrajectorySegment *ns;
  71     Point *np = NULL;
  72     if (typeid(**seg) != typeid(Turn))
  73         np = new Point;
  74
  75     ns = (*seg)->splitAt(distance, np);
  76     if (ns == NULL) {
  77         dbgPrintf("ERROR\n");
  78         delete(np);
  79         return false;
  80     }
  81     if (np)
  82         wayPoints.push_back(np);
  83     seg = insert(++seg, ns);
  84     return true;
  85 }
  86
  87 //     iterator insert(reverse_iterator it, const value_type& val) {
  88 //         return insert((--it).base(), val);
  89 //     }
  90
  91 /**
  92  * Connects points by line segments.
  93  */
  94 bool
  95 Trajectory::points2segments()
  96 {
  97     TrajectoryPoints::iterator p1, p2, p3;
  98     Point ip(initPos.x, initPos.y);
  99     double edge;
 100
 101     if (*wayPoints.front() != ip) {
 102         initialPoint = new Point(ip);
 103         wayPoints.push_front(initialPoint);
 104     } else
 105         initialPoint = wayPoints.front();
 106
 107
 108     // Delete old segments
 109     deleteSegments();
 110
 111     if (wayPoints.size() < 2 &&
 112         (finalHeading.turn_type == FH_DONT_TURN ||
 113          (finalHeading.turn_type == FH_SHORTEST && \
 114           fabs(initPos.phi - finalHeading.heading) < 1.0/180*M_PI))) {
 115         return false;
 116     }
 117
 118     if (wayPoints.size() >= 3) {
 119         // be aware of two identical points, you can not measure any angle, so delete one of them
 120         for (p1 = wayPoints.begin(), p2 = ++wayPoints.begin(); p2 != wayPoints.end(); p1++, p2++) {
 121             if ((*p1)->distanceTo(**p2) < 0.001) {
 122                 #ifdef MOTION_LOG
 123                 char ccc[150];
 124                 sprintf(ccc, "One of identical points x %lf y %lf erased\n", (*p2)->x, (*p2)->y);
 125                 log(ccc);
 126                 #endif
 127                 p2 = wayPoints.erase(p2);
 128                 }
 129         }
 130
 131         // For avoiding too sharp turns than a constant we add one point to the right angle, so that
 132         // we obtain two bigger angles instead of one very small. This point is so close to the original one
 133         // and should not affect the trajectory from the macroscopical point of view.
 134         for (p1 = wayPoints.begin(), p2 = ++wayPoints.begin(), p3 = ++wayPoints.begin(), ++p3;
 135              p3 != wayPoints.end();
 136              p1++, p2++, p3++) {
 137             edge = fabs((*p2)->angleTo(**p1) - (*p2)->angleTo(**p3));
 138             if (edge > M_PI)
 139                 edge =  (2*M_PI) - edge;
 140             if (edge*180/M_PI < 3.0)
 141                 {
 142                     edge = (*p2)->angleTo(**p1) + M_PI/2.0;
 143                     Point *pt = new Point ((*p2)->x + cos(edge)*0.025, (*p2)->y + sin(edge)*0.025);
 144                     wayPoints.insert(p2, pt);
 145                     #ifdef MOTION_LOG
 146                     char ccc[150];
 147                     sprintf(ccc, "Sharp edge smoothed %lf %lf, %lf %lf, %lf %lf\n",
 148                         (**p1).x, (**p1).y, (**p2).x, (**p2).y, (**p3).x, (**p3).y);
 149                     log(ccc);
 150                     #endif
 151                 }
 152         }
 153     }
 154
 155     #ifdef MOTION_LOG
 156     char c[200];
 157     for (p1 = wayPoints.begin(); p1 != wayPoints.end(); p1++) {
 158         sprintf(c, "wayPoints: x %lf y %lf\n", (*p1)->x, (*p1)->y);
 159         log(c);
 160     }
 161     #endif
 162
 163     for (p1 = wayPoints.begin(), p2 = ++wayPoints.begin();
 164          p2 != wayPoints.end();
 165          p1++, p2++) {
 166         push_back(new Line(*p1, *p2));
 167     }
 168     return true;
 169 }
 170
 171
 172 /**
 173  * Converts corners in trajectory to arcs.
 174  */
 175
 176 // \f{center}
 177 // \frame{\begin{tikzpicture}
 178 //  \draw (-5cm,0) coordinate(p1) node[left]{$p_1$} --
 179 //   node[sloped,above]{$\mathrm seg_1$} (0,0) coordinate(p2)
 180 //   node[above]{$p_2$} -- node[sloped,above]{$\mathrm seg_2$} +(-30:5cm)
 181 //   node[right]{$p_3$} coordinate (p3);
 182 //   \coordinate[shift=(-105:5cm)] (c) at (p2);
 183 //   \draw (p2) -- (c);
 184 //   \draw (c) -- (c |- p2);
 185 //   \draw (c) -- (intersection of c--40:10cm and p2--p3);
 186 // \end{tikzpicture}}
 187 //    \f}
 188
 189 /*
 190 void
 191 Trajectory::corners2arcs()
 192 {
 193     iterator seg, segNext, segPrev;
 194
 195     if (size() < 2) return;
 196
 197     // Find radiuses to meet e <= maxe
 198     for (segPrev = begin(), segNext = ++begin();
 199          segNext != end();
 200          segPrev++, segNext++) {
 201
 202         double alpha;       // curve angle (-pi <= alpha <= +pi)
 203         double r;           // radius of proposed arc. (alpha < 0 -> r < 0)
 204         double s;           // length of the segment replaced by the arc (s > 0)
 205         try {
 206             alpha = dynamic_cast<Line*>(*segNext)->getAngle() -
 207                 dynamic_cast<Line*>(*segPrev)->getAngle();
 208             if (alpha > +M_PI) alpha -= 2.0*M_PI;
 209             if (alpha < -M_PI) alpha += 2.0*M_PI;
 210
 211             if (fabs(alpha) < M_PI/180.0) continue;
 212
 213             r = constr.maxe / (1/cos(alpha/2.0) - 1);
 214             if (alpha < 0) r = -r;
 215             s = r*tan(alpha/2.0);
 216             if (s > (*segPrev)->getLength()) {
 217                 s = (*segPrev)->getLength();
 218                 r = s/tan(alpha/2.0);
 219             }
 220             if (s > (*segNext)->getLength()) {
 221                 s = (*segNext)->getLength();
 222                 r = s/tan(alpha/2.0);
 223             }
 224             (*segPrev)->r = r;
 225             (*segPrev)->s = s;
 226             (*segPrev)->alphahalf = alpha/2.0;
 227         }
 228         catch (std::exception& e) {} // Skip wrong dynamic_cast
 229     }
 230
 231     if (size() >= 3) {
 232         // Reduce overlapping arcs by decreasing radiuses
 233         double s1, s2, r1, r2, l, alphahalf1, alphahalf2;
 234         bool wantSameR;
 235         for (segPrev = begin(), segNext = ++begin();
 236              segNext != --end();
 237              segPrev++, segNext++) {
 238             l = (*segNext)->getLength();
 239             s1 = (*segPrev)->s;
 240             s2 = (*segNext)->s;
 241             r1 = (*segPrev)->r;
 242             r2 = (*segNext)->r;
 243             alphahalf1 = (*segPrev)->alphahalf;
 244             alphahalf2 = (*segNext)->alphahalf;
 245             if (s1 + s2 > l) {
 246                 if (fabs(r1) > fabs(r2)) {
 247                     s1 = l - s2;
 248                     r1 = s1/tan(alphahalf1);
 249                     wantSameR = (fabs(r1) < fabs(r2));
 250                 } else {
 251                     s2 = l - s1;
 252                     r2 = s2/tan(alphahalf2);
 253                     wantSameR = (fabs(r1) > fabs(r2));
 254                 }
 255                 if (wantSameR) {
 256                     if (alphahalf1 < 0 ^ alphahalf2 < 0)
 257                         s1 = l*tan(alphahalf1)/(tan(alphahalf1)-tan(alphahalf2));
 258                     else
 259                         s1 = l*tan(alphahalf1)/(tan(alphahalf1)+tan(alphahalf2));
 260                     s2 = l - s1;
 261                     r1 = s1/tan(alphahalf1);
 262                     r2 = s2/tan(alphahalf2);
 263                 }
 264                 (*segPrev)->s = s1;
 265                 (*segNext)->s = s2;
 266                 (*segPrev)->r = r1;
 267                 (*segNext)->r = r2;
 268             }
 269         }
 270     }
 271
 272     // replace corners by arcs with previously computed radiuses
 273     for (segPrev = begin(), segNext = ++begin();
 274          segNext != end();
 275          ++segPrev, ++segNext) {
 276         if (fabs((*segPrev)->r) > 0) {
 277             Point *newEnd1, *newEnd2;
 278             double s = (*segPrev)->s;
 279             newEnd1 = new Point();
 280             newEnd2 = new Point();
 281             (*segPrev)->shortenBy(+s, newEnd1);
 282             (*segNext)->shortenBy(-s, newEnd2);
 283             wayPoints.push_back(newEnd1);
 284             wayPoints.push_back(newEnd2);
 285             segPrev = insert(segNext, new Arc(newEnd1, newEnd2,
 286                                                         (*segPrev)->r));
 287         }
 288     }
 289
 290     //delete zero-length segments
 291     seg = begin();
 292     while (seg != end()) {
 293         if ((*seg)->getLength() <= 1e-9) {
 294             delete(*seg);
 295             seg = erase(seg);
 296         } else
 297             seg++;
 298     }
 299 }
 300 */
 301
 302 /**
 303  * Converts corners in trajectory to splines.
 304  * The algorithm finds the smaller of two neighbour lines,
 305  * grabs a half of it's length, than approximation constraint shortens this distance.
 306  * This distance is replaced by a symetric spline curve.
 307  */
 308 void
 309 Trajectory::corners2splines()
 310 {
 311 #ifdef MATLAB_MEX_FILE
 312         mexEvalString("disp('...preparing spline...')");
 313 #endif
 314
 315     iterator seg, segNext, segPrev;
 316     double origLen = -1.0; // length of segment segPrev before being cut off
 317
 318     if (size() < 2) return;
 319
 320     for (segPrev = begin(), segNext = ++begin();
 321          segNext != end();
 322          ++segPrev, ++segNext)
 323     {
 324         Point *newEnd1, *newEnd2, *edge;
 325         edge    = new Point();
 326         newEnd1 = new Point();
 327         newEnd2 = new Point();
 328
 329         double dst; // distance to cut off from both Lines
 330
 331         if (origLen == -1)
 332             dst = fmin((*segPrev)->getLength(), (*segNext)->getLength()) * 0.5;
 333         else
 334             dst = fmin(origLen, (*segNext)->getLength()) * 0.5;
 335
 336         // just to count the angle between lines
 337         (*segPrev)->getPointAt((*segPrev)->getLength(), edge);
 338         (*segPrev)->getPointAt(-dst, newEnd1);
 339         (*segNext)->getPointAt(dst, newEnd2);
 340
 341         // shorten dst according to constraint maxe
 342         double ang = fabs(edge->angleTo(*newEnd1) - edge->angleTo(*newEnd2));
 343         if (ang > M_PI)
 344             ang =  (2.0*M_PI) - ang;
 345         double e;  // distance from the spline to edge
 346         const double C0 = 0.892303;
 347         const double C1 = -0.008169;
 348         const double C2 = 1.848555E-05;
 349
 350         // experimental relation, but good enough
 351         // see Matlab m-file 'splines/constr_maxe.m'
 352         e = (C2*pow(ang*180.0/M_PI, 2) + C1*(ang*180.0/M_PI) + C0) * dst;
 353
 354         if (e > constr.maxe && !(segPrev == begin() && contains_inertial))
 355             dst = dst * (constr.maxe/e);
 356
 357         origLen = (*segNext)->getLength();
 358
 359         (*segPrev)->getPointAt((*segPrev)->getLength(), edge);
 360         (*segPrev)->shortenBy(+dst, newEnd1);
 361         (*segNext)->shortenBy(-dst, newEnd2);
 362         wayPoints.push_back(newEnd1);
 363         wayPoints.push_back(newEnd2);
 364         segPrev = insert(segNext, new Spline(newEnd1, newEnd2, edge));
 365         #ifdef MOTION_LOG
 366         char c[200];
 367         sprintf(c, "splineToBeCounted: begin %lf %lf end %lf %lf edge %lf %lf\n",
 368                 newEnd1->x, newEnd1->y, newEnd2->x, newEnd2->y, edge->x, edge->y);
 369         log(c);
 370         #endif
 371     }
 372
 373     #ifdef MOTION_LOG
 374     TrajectoryPoints::iterator p1;
 375     char c[200];
 376     for (p1 = wayPoints.begin(); p1 != wayPoints.end(); p1++) {
 377         sprintf(c, "wayPointsSplined: x %lf y %lf\n", (*p1)->x, (*p1)->y);
 378         log(c);
 379     }
 380     #endif
 381
 382     //delete zero-length segments
 383     seg = begin();
 384     while (seg != end()) {
 385
 386         if ((*seg)->getLength() <= 1e-9) {
 387             delete(*seg);
 388             seg = erase(seg);
 389         } else
 390             seg++;
 391     }
 392
 393 }
 394
 395
 396 /**
 397  * Assigns speeds and accelerations to segments; tries to maximize
 398  * speed where possible. If necessary it also splits long segments
 399  * to multiple parts with maximum acceleration/deceleration and
 400  * maximal speed. The optimization is done in three passes. In the
 401  * first pass, maximal speed of segments are set up (see
 402  * TrajectoryConstraints::maxv, TrajectoryConstraints::maxomega,
 403  * TrajectoryConstraints::maxcenacc). Then follows a forward pass, where
 404  * acceleration is added to join segments with increasing maximal
 405  * speeds. Finally, backward pass does almost the same by adding
 406  * deceleration segments.
 407  *
 408  * Finding the split point of the segment where deceleration
 409  * immediately follows acceleration is based on the following
 410  * calculations:
 411  *
 412  * For uniformly accelerated motion, we can calculate time as
 413  * function of speed: \f[ t(v)=\frac{v-v_0}{a}\f] Using this, we can
 414  * express distance \f$ s \f$ as a function of speed: \f[ s(v) =
 415  * \frac{1}{2} at(v)^2 + v_0 t(v) = \frac{v^2 - v_0^2}{2a}\f]
 416  *
 417  * Then, distance traveled during acceleration (\f$ s_1 \f$) and
 418  * deceleration (\f$ s_2 \f$) must be equal to the length of the
 419  * segment \f$ l = s_1 + s_2 \f$. Considering a segment with initial
 420  * speed \f$ v_1 \f$, final speed \f$ v_2 \f$, and maximal speed (in
 421  * the split point) \f$ v_{max} \f$, we can derive: \f[ l =
 422  * s_1(v_{max}) + s_2(v_{max}) = \frac{2v_{max}^2 - v_1^2 - v_2^2}{2a}
 423  * \f] \f[ v_{max}^2 = al + \frac{v_1^2 + v_2^2}{2} \f]
 424  *
 425  * Then, the length of the segment before split is: \f[ s_1 = l - s_2
 426  * = l - \frac{v_{max}^2-v_2^2}{2a} = \frac{1}{2}l + \frac{v_2^2 -
 427  * v_1^2}{4a} \f]
 428  *
 429  * --- SPLINE UPDATE: The algorithm works nearly the same as before and is
 430  * possible to use with splines. However, spline segments are not being
 431  * cut off, but only their acceleration profiles change to ensure speed
 432  * connectivity. This is not time optimal, but time loss is small enough,
 433  * we avoid a lot of difficulties and keep regressive compatibility (with arcs).
 434  */
 435 void
 436 Trajectory::calcSpeeds()
 437 {
 438     if (size() == 0) return;
 439
 440     // Assign maximum speed
 441     for (iterator seg = begin(); seg != end(); seg++) {
 442         (*seg)->setMaxV(constr);
 443 //        (*seg)->v1 = (*seg)->maxv;
 444 //        (*seg)->v2 = (*seg)->maxv;
 445     }
 446 #ifdef MATLAB_MEX_FILE
 447     plotSpeed("r-o");
 448 #endif
 449
 450     double lastv;
 451     bool turning = false;       // FIXME: turning je integer podle smeru otaceni
 452
 453     // Accelerate where possible
 454     lastv = initPos.v;
 455     for (iterator seg = begin(); seg != end(); seg++) {
 456         if (turning != (*seg)->isTurn()) {
 457             turning = (*seg)->isTurn();
 458             lastv = 0;
 459         }
 460         double v1 = (*seg)->v1;
 461         double v2 = (*seg)->v2;
 462         double maxv = (*seg)->maxv;
 463         //dbgPrintf("processing: %p v1=%g\n", *seg, v1);
 464         if (v1 > lastv) {
 465             v1 = lastv;
 466
 467             if ((*seg)->isSpline() == false) {
 468                 double l = (*seg)->getUnifiedLength();
 469                 double a = (*seg)->getMaxAcc(constr);
 470                 double t;
 471
 472                 t = (-v1+sqrt(v1*v1 + 2.0*a*l))/a;
 473                 v2 = v1+a*t;
 474
 475                 //dbgPrintf("t=%g v2=%g l=%g\n", t, v2, l);
 476                 if (v2 > maxv) {        // split this segment
 477                     v2 = maxv;
 478                     t = (v2 - v1)/a;
 479                     l = v1*t+0.5*a*t*t;
 480                     //dbgPrintf("t=%g v2=%g l=%g\n", t, v2, l);
 481                     iterator ns = seg;
 482                     if (splitSegment(ns, l)) {
 483                         (*ns)->v1 = v2;
 484                     }
 485                 }
 486             }
 487
 488             (*seg)->v1 = v1;
 489             (*seg)->v2 = v2;
 490         }
 491         lastv = (*seg)->v2;
 492     }
 493 #ifdef MATLAB_MEX_FILE
 494     plotSpeed("g-o");
 495 #endif
 496
 497     // Deccelerate where needed
 498     turning = false;
 499     lastv = 0;              // Final speed
 500     for (reverse_iterator seg = rbegin(); seg != rend(); seg++) {
 501         if (turning != (*seg)->isTurn()) {
 502             turning = (*seg)->isTurn();
 503             lastv = 0;
 504         }
 505         double v1 = (*seg)->v1;
 506         double v2 = (*seg)->v2;
 507         double maxv = (*seg)->maxv;
 508         dbgPrintf("processing: %p v2=%g\n", *seg, v2);
 509         if (v2 > lastv) {
 510             v2 = lastv;
 511
 512             if ((*seg)->isSpline() == false) {
 513                 double l = (*seg)->getUnifiedLength();
 514                 double a = (*seg)->getMaxAcc(constr);
 515                 double t;
 516
 517                 t = (-v2+sqrt(v2*v2 + 2.0*a*l))/a;
 518                 v1 = v2+a*t;
 519
 520                 //dbgPrintf("t=%g v1=%g v2=%g l=%g\n", t, v1, v2, l);
 521                 if (v1 > maxv && (*seg)->v1 == maxv) {
 522                     v1 = maxv;
 523                     t = (v1 - v2)/a;
 524                     double l2 = v2*t+0.5*a*t*t;
 525                     //dbgPrintf("t=%g v1=%g v2=%g l=%g l2=%g\n", t, v1, v2, l, l2);
 526                     iterator ns = --(seg.base());
 527                     //dbgPrintf("seg=%p ns=%p\n", *seg, *ns);
 528                     if (splitSegment(ns, l-l2)) {
 529                         //dbgPrintf("ns=%p\n", *ns);
 530                         (*ns)->v2 = v1;
 531                         (*seg)->v1 = v1;
 532                     }
 533                 } else if (v1 > (*seg)->v1) {
 534                     // Acceleration and deacceleration within one segment
 535                     // Where is the crossing af acceleration and decceleration?
 536                     v1 = (*seg)->v1;
 537                     double s1 = l/2.0 + (v2*v2 - v1*v1)/4.0*a;
 538
 539                     if (s1 > l/1000 && s1 < l - l/1000) {
 540                         //dbgPrintf("t=%g v1=%g v2=%g l=%g s1=%g\n", t, v1, v2, l, s1);
 541                         iterator ns = --(seg.base());
 542                         //dbgPrintf("seg=%p ns=%p (before split)\n", *seg, *ns);
 543                         if (splitSegment(ns, s1)) {
 544                             ++seg; //move to the earlier subsegment of the splited one
 545                             //dbgPrintf("seg=%p ns=%p (after split)\n", *seg, *ns);
 546                             v1 = sqrt(v1*v1 + 2.0*a*s1);
 547                             (*ns)->v1 = v1;
 548                             (*ns)->v2 = v2;
 549                             v2 = v1;
 550                             v1 = (*seg)->v1;
 551                         }
 552                     }
 553                 } else {
 554                     (*seg)->v1 = v1;
 555                 }
 556             }
 557             (*seg)->v2 = v2;
 558         }
 559         lastv = (*seg)->v1;
 560     }
 561 #ifdef MATLAB_MEX_FILE
 562     plotSpeed("b-o");
 563 #endif
 564     for (iterator seg = begin(); seg != end(); ++seg) {
 565         dbgPrintf("final: %-16s %p v1=%-8.2g v2=%-8.2g l=%-8.2g\n",
 566                   typeid(**seg).name(),
 567                   *seg, (*seg)->v1, (*seg)->v2, (*seg)->getLength());
 568     }
 569 }
 570
 571
 572 double
 573 Trajectory::calcLength()
 574 {
 575     double totLen = 0;
 576     for (iterator seg = begin(); seg != end(); ++seg) {
 577         totLen += (*seg)->getLength();
 578     }
 579     return totLen;
 580 }
 581
 582 static double minimizeAngle(double rad)
 583 {
 584     rad = fmod(rad, 2.0*M_PI);
 585     if (rad > +M_PI) rad -= 2.0*M_PI;
 586     if (rad < -M_PI) rad += 2.0*M_PI;
 587     return rad;
 588 }
 589
 590 static double calcTurnBy(double initialHeading, struct final_heading finalHeading)
 591 {
 592         double turnBy;
 593         if (finalHeading.turn_type == FH_DONT_TURN) turnBy = NAN;
 594         else {
 595                 turnBy = minimizeAngle(finalHeading.heading - initialHeading);
 596                 if (turnBy < 0 && finalHeading.turn_type == FH_CCW) turnBy += M_PI*2.0;
 597                 if (turnBy > 0 && finalHeading.turn_type == FH_CW) turnBy -= M_PI*2.0;
 598         }
 599         return turnBy;
 600 }
 601
 602 /**
 603  *  Adds a point to the beginning of trajectory, because moving robot can not change
 604  *  direction at once. Needed by nonzero initial speed.
 605  */
 606 void Trajectory::addInertialPoint()
 607 {
 608     Point p;
 609     double braking_distance;
 610     braking_distance = (0.5 * initPos.v * initPos.v / constr.maxacc) + 0.02;
 611     p.x = initPos.x + braking_distance * cos(initPos.phi);
 612     p.y = initPos.y + braking_distance * sin(initPos.phi);
 613         initialPoint = new Point(p);
 614     wayPoints.push_front(initialPoint);
 615     contains_inertial = true;
 616 }
 617
 618
 619 void Trajectory::addTurns()
 620 {
 621     Pos p;
 622     TrajectorySegment *seg;
 623     double initialHeading = initPos.phi;
 624     double turnBy;
 625     if (backward)
 626         initialHeading -= M_PI;
 627
 628     if (size() == 0) {
 629         // We have to turn only. No movement.
 630         dbgPrintf("*** Adding only the turn - start: %5.2f, end %5.2f\n", initialHeading/M_PI*180, finalHeading.heading/M_PI*180);
 631         turnBy = calcTurnBy(initialHeading, finalHeading);
 632         if (fabs(turnBy) > 1e-3) {
 633             push_front(new Turn(initialPoint, initialHeading, turnBy));
 634         }
 635     } else {
 636         // Add turn at beginning
 637         seg = *begin();
 638         seg->v1 = 1;           // Hack - speeds are not calculated yet
 639         seg->v2 = 1;
 640         seg->startAt(0);        // Assign times so we can use
 641                                 // getRefPos();
 642         seg->getRefPos(0, p);
 643         dbgPrintf("*** Adding initial turn - start: %5.2f, end %5.2f\n", initialHeading/M_PI*180, p.phi/M_PI*180);
 644         turnBy = minimizeAngle(p.phi - initialHeading);
 645         if (fabs(turnBy) > 1e-3 && initPos.v == 0) {
 646             push_front(new Turn(initialPoint, initialHeading, turnBy));
 647         }
 648
 649         // Add turn at the end
 650         seg = *(--end());
 651         seg->v1 = 1;           // Hack - speeds are not calculated yet
 652         seg->v2 = 1;
 653         seg->startAt(0);            // Assign times so we can use getRefPos();
 654         seg->getRefPos(seg->getT2(), p);
 655         turnBy = calcTurnBy(p.phi, finalHeading);
 656         if (fabs(turnBy) > 1e-3) { // NaN in finalHeading makes the condition false
 657             dbgPrintf("*** Adding final turn - start: %5.2f, end %5.2f\n", p.phi/M_PI*180, finalHeading.heading/M_PI*180);
 658             push_back(new Turn(finalPoint, p.phi, turnBy));
 659         }
 660     }
 661 }
 662
 663 /**
 664  * Prepares the trajectory. It converts points added by addPoint() to
 665  * line segments (::Line). Then it replaces by sharp
 666  * corners by arcs (see corners2arcs()), then it adds turns at the
 667  * begin and end and finally it calculates speeds in the different
 668  * parts of the trajectory by adding acceleration and decceleration
 669  * segments (see calcSpeeds()).
 670  *
 671  * @param _initPos The point where the trajectory should
 672  * start. Typically the current robot position.
 673  * @return True in case of success, false when the trajectory has only
 674  * one point and initPos is already equal to that point.
 675  */
 676 bool
 677 Trajectory::prepare(Pos _initPos)
 678 {
 679     if (constr.maxv      <= 0 || constr.maxv      > 10 ||
 680         constr.maxomega  <= 0 || constr.maxomega  > 10 ||
 681         constr.maxangacc <= 0 || constr.maxangacc > 10 ||
 682         constr.maxacc    <= 0 || constr.maxacc    > 10 ||
 683         constr.maxcenacc <= 0 || constr.maxcenacc > 10 ||
 684         constr.maxe      <= 0 || constr.maxe      > 10) {
 685         dbgPrintf("wrong constraints!!!!\n");
 686     }
 687
 688     if (wayPoints.size() == 0) {
 689         dbgPrintf("No point in trajectory!\n");
 690         return false;
 691     }
 692
 693     initPos = _initPos;
 694
 695     contains_inertial = false;
 696     // adds a point in actual (initial) direction
 697     if (initPos.v != 0)
 698         addInertialPoint();
 699
 700     // TODO: no support for omega continuity yet
 701     if (initPos.v == 0 && initPos.omega != 0)
 702     {
 703 //        printf("//// not supported turning\n");
 704         initPos.omega = 0;
 705     }
 706
 707
 708     if (points2segments() == false) {
 709         dbgPrintf("points2segments error!\n");
 710         return false;
 711     }
 712
 713 //    corners2arcs();
 714     corners2splines();
 715     addTurns();
 716     if (size() == 0)
 717         return false;
 718     calcSpeeds();
 719
 720     // Assign times to segments
 721     double t=0;
 722     for (iterator seg = begin(); seg != end(); ++seg) {
 723         t = (*seg)->startAt(t);
 724     }
 725     currentSeg = begin();
 726     prepared = true;
 727
 728     return true;
 729 }
 730
 731 /**
 732  * Returns reference position of the trajectory at a given time.
 733  *
 734  * @param[in]  time Time from the beginning of the trajectory.
 735  * @param[out] rp  Returned reference position.
 736  * @return True if the time is after the trajectory ends or
 737  *         error. False otherwise.
 738  */
 739 bool
 740 Trajectory::getRefPos(double time, Pos &rp)
 741 {
 742     int d;
 743     bool ret = false;
 744     TrajectorySegment *seg;
 745
 746     if (!prepared) {
 747         ret = true;
 748     } else {
 749         do {
 750             d=(*currentSeg)->containsTime(time);
 751             if (d==0) break;
 752             if (currentSeg == --end() && d > 0) break;
 753             if (currentSeg == begin() && d < 0) break;
 754             if (d>0) ++currentSeg;
 755             else if (d<0) --currentSeg;
 756         } while (1);
 757
 758         seg = *currentSeg;
 759         #ifdef MOTION_LOG
 760         char c[100];
 761         sprintf(c, "slength %lf\n", seg->getLength()); // gnuplot middle part
 762         log(c);
 763         #endif
 764         if (d==0)
 765             getSegRefPos(seg, time, rp);
 766         else if (d<0)
 767             getSegRefPos(seg, seg->getT1(), rp);
 768         else {
 769             getSegRefPos(seg, seg->getT2(), rp);
 770             ret = true;             // We are at the end.
 771         }
 772     }
 773
 774     return ret;
 775 }
 776
 777 /**
 778  * Returns reference position of the given segment in the trajectory
 779  * and takes @c backward into the account.
 780  *
 781  * @param[in]  seg Segment to get the position from.
 782  * @param[in]  time Time from the beginning of the trajectory.
 783  * @param[out] rp  Returned reference position.
 784  */
 785 void
 786 Trajectory::getSegRefPos(TrajectorySegment *seg, double time, Pos &rp)
 787 {
 788     seg->getRefPos(time, rp);
 789
 790     if (backward) {
 791         rp.phi += M_PI;
 792         rp.v = -rp.v;
 793     }
 794 }
 795
 796 /**
 797  * Connects to the recent trajectory a new one - @c traj.
 798  * It must satisfy some demands e.g. being prepared.
 799  * @param traj new trajectory
 800  * @param time on the recent trajectory to find the point where to join
 801  * trajectories.
 802  * @return true if the action succeeded
 803  */
 804 bool Trajectory::appendTrajectory(Trajectory &traj, double time)
 805 {
 806         if (!traj.prepared)
 807                 return false;
 808         if (backward != traj.backward)
 809                 return false;
 810
 811         prepared = false;
 812
 813 //        printf("swtime> %lf\n", time);
 814
 815 /*        Pos h1, h2, h3, h4;
 816         for (iterator seg = begin(); seg != end(); ++seg) {
 817                 (*seg)->getRefPos((*seg)->getT1(), h1);
 818                 (*seg)->getRefPos((*seg)->getT2(), h2);
 819                 printf("puvodni %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
 820                 printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
 821                 printf("details: v1 %lf v2 %lf acc %lf len %lf\n", (*seg)->v1, (*seg)->v2, (*seg)->acc, (*seg)->getLength()); // segment details
 822         }
 823
 824         for (iterator seg = traj.begin(); seg != traj.end(); ++seg) {
 825                 (*seg)->getRefPos((*seg)->getT1(), h1);
 826                 (*seg)->getRefPos((*seg)->getT2(), h2);
 827                 printf("novy   %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
 828                 printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
 829         }
 830 */
 831         // split & delete
 832         for (iterator seg = begin(); seg != end(); ++seg) {
 833                 if ((*seg)->containsTime(time) == 0) {
 834                         Pos con_pos;
 835                         (*seg)->getRefPos(time, con_pos);
 836                         Point continuity(con_pos.x, con_pos.y);
 837                         if (continuity != *(traj.initialPoint)) {
 838                                 return false;   // the trajectories don't fit
 839                         }
 840 //                      if (time == (*seg)->getT1()) {
 841 //                              seg = erase(seg, end());
 842 //                      } else if (time == (*seg)->getT2()) {
 843 //                              seg++;
 844 //                              if (seg != end())
 845 //                                      seg = erase(seg, end());
 846 //                      } else {
 847                                 splitSegmentByTime(seg, time);
 848                                 seg = erase(seg, end());
 849 //                      }
 850
 851                         break;
 852                 }
 853         }
 854
 855         // add new segments to the old trajectory
 856         //merge(traj);
 857         splice(end(), traj);
 858
 859         // add way points (why?)
 860         // final point
 861         finalPoint = traj.finalPoint;
 862
 863         // final heading
 864         finalHeading = traj.finalHeading;
 865
 866         // assign times
 867         double t=0;
 868         for (iterator seg = begin(); seg != end(); ++seg) {
 869                 t = (*seg)->startAt(t);
 870         }
 871
 872         prepared = true;
 873 /*
 874         for (iterator seg = begin(); seg != end(); ++seg) {
 875                 (*seg)->getRefPos((*seg)->getT1(), h1);
 876                 getRefPos((*seg)->getT1(), h3);
 877                 (*seg)->getRefPos((*seg)->getT2(), h2);
 878                 getRefPos((*seg)->getT2(), h4);
 879                 printf("merged  %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
 880                 printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
 881                 printf("*merged  %lf %lf    %lf %lf\n", h3.x, h3.y, h4.x, h4.y);
 882                 printf("details: v1 %lf v2 %lf acc %lf len %lf\n", (*seg)->v1, (*seg)->v2, (*seg)->acc, (*seg)->getLength()); // segment details
 883         }
 884 */
 885         currentSeg = begin();
 886         prepared = true;
 887
 888 //        Pos p;
 889 //        getRefPos(0.1, p);
 890 //      printf("-difference real %lf %lf\n", p.x, p.y);
 891
 892         #ifdef MOTION_LOG
 893         log("trajectory APPENDED (merged two trajectories togeather)\n");
 894
 895         char c[200];
 896         Pos p1, p2;
 897         for (iterator seg = begin(); seg != end(); seg++) { // logging segment parameters
 898                 (*seg)->getRefPos((*seg)->getT1(), p1);
 899                 (*seg)->getRefPos((*seg)->getT2(), p2);
 900                 sprintf(c, "segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n",
 901                         p1.x, p1.y, p2.x, p2.y, p1.phi, p2.phi,(*seg)->v1, (*seg)->v2, (*seg)->getT1(), (*seg)->getT2(), (*seg)->getLength());
 902                 log(c);
 903         }
 904         iterator end_seg = --end();
 905         double max_tm = (*end_seg)->getT2();
 906         // for gnuplot
 907         for(double tm = 0; tm <= max_tm; tm += 0.05) {
 908                 getRefPos(tm, p1);
 909                 sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", tm, p1.x, p1.y, p1.phi, 2); // gnuplot middle part
 910                 log(c);
 911                 }
 912         #endif
 913         return true;
 914 }
 915
 916
 917 void
 918 Trajectory::log(const char* str) {
 919         FILE *f;
 920         f = fopen("motion.log", "a");
 921         if (f == NULL) {
 922                 printf("logged not - cant open/create motion.log");
 923                 return;
 924         }
 925         fprintf(f, str);
 926         fclose(f);
 927 }
 928
 929 #ifdef MATLAB_MEX_FILE
 930 void
 931 Trajectory::plot(const char *style)
 932 {
 933     for (iterator seg = begin(); seg != end(); ++seg) {
 934         (*seg)->plot(style);
 935     }
 936 };
 937 void
 938 Trajectory::plotSpeed(const char *style)
 939 {
 940     const char *f2 = "figure(2); xlabel('Time [s]'); ylabel('Speed [m/s], dotted [rad/s]');";
 941     const char *f1 = "hold on; figure(1)";
 942     char turnstyle[20];
 943     strcpy(turnstyle, style);
 944     if (turnstyle[1] == '-') turnstyle[1] = ':';
 945     double t = 0;
 946     mexEvalString(f2);
 947     for (iterator seg = begin(); seg != end(); ++seg) {
 948         t = (*seg)->startAt(t);
 949         if (!(*seg)->isTurn())
 950             (*seg)->plotSpeed(style);
 951         else
 952             (*seg)->plotSpeed(turnstyle);
 953     }
 954     mexEvalString(f1);
 955
 956 /*    // debuging info
 957       char debug [200];
 958       int i = 0;
 959       t=0;
 960       for (iterator seg = begin(); seg != end(); ++seg) {
 961           sprintf(debug, "disp('*** seg %d: t1 %lf, t2 %lf, len %lf, v1 %lf, v2 %lf.')",
 962                   i, t, (*seg)->startAt(t), (*seg)->getUnifiedLength(), (*seg)->v1, (*seg)->v2);
 963           t = (*seg)->startAt(t);
 964           i++;
 965           mexEvalString(debug);
 966       }
 967 */
 968 };
 969 #endif
 970
 971 #ifdef MOTION_LOG
 972 void
 973 Trajectory::logTraj(double start_time)
 974 {
 975     char c[200];
 976     Pos p1, p2;
 977     for (iterator seg = begin(); seg != end(); seg++) { // logging segment parameters
 978         (*seg)->getRefPos((*seg)->getT1(), p1);
 979         (*seg)->getRefPos((*seg)->getT2(), p2);
 980         sprintf(c, "segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n",
 981                 p1.x, p1.y, p2.x, p2.y, p1.phi, p2.phi,(*seg)->v1, (*seg)->v2, (*seg)->getT1(), (*seg)->getT2(), (*seg)->getLength());
 982         log(c);
 983         sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", start_time+(*seg)->getT1(), p1.x, p1.y, p1.phi, 1); //gnuplot start point
 984         log(c);
 985         sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", start_time+(*seg)->getT2(), p2.x, p2.y, p2.phi, 3); //gnuplot end point
 986         log(c);
 987         }
 988     iterator end_seg = --end();
 989     double max_tm = (*end_seg)->getT2();
 990     // for gnuplot
 991     for(double tm = 0; tm <= max_tm; tm += 0.05) {
 992         getRefPos(tm, p1);
 993         sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", start_time+tm, p1.x, p1.y, p1.phi, 2); // gnuplot middle part
 994         log(c);
 995         }
 996 }
 997 #endif