src/motion/trajectory.cc

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