src/motion/trgen.h

   1 // -*- c++ -*-
   2
   3 #ifndef __TRGEN_H
   4 #define __TRGEN_H
   5
   6 /**
   7  * @file   trgen.h
   8  * @author Michal Sojka
   9  * @date   Sun Aug 12 09:30:59 2007
  10  *
  11  * @brief  Trajectory Generator
  12  */
  13
  14 /** \defgroup trgen Trajectory Generator
  15  @{
  16
  17  Trajectory generator calculates the trajectory and speed profile for
  18  the robot movement from a list of points. For how to use the
  19  trajectory generator, see the documentation of ::Trajectory.
  20  */
  21 #ifdef MATLAB_MEX_FILE
  22 #include <mex.h>
  23 #endif
  24
  25 #include <list>
  26 #include <math.h>
  27 #include "trgenconstr.h"
  28
  29 /**
  30  * Represents a state of the robot.
  31  */
  32 class Pos {
  33 public:
  34     double x, y, phi;           ///< position and angle
  35     double v, omega;            ///< forward and angular speed
  36     Pos() : x(0), y(0), phi(0), v(0), omega(0) {};
  37 };
  38
  39 /**
  40  * Represents a point in 2D space and provides some useful methods for
  41  * calculation with points.
  42  */
  43 class Point {
  44 public:
  45     double x, y;
  46     Point() : x(0), y(0) {};
  47     Point(double _x, double _y) : x(_x), y(_y) {};
  48
  49     double distanceTo(class Point &wp) {
  50         double dx = wp.x-x, dy = wp.y-y;
  51         return sqrt(dx*dx + dy*dy);
  52     }
  53
  54     double angleTo(class Point &wp) {
  55         double dx = wp.x-x, dy = wp.y-y;
  56         return atan2(dy,dx);
  57     }
  58
  59     bool operator==(Point &o) {
  60         return (x == o.x && y == o.y);
  61     }
  62
  63     bool operator!=(Point &o) {
  64         return !(*this == o);
  65     }
  66 };
  67
  68 /**
  69  * List of pointers to Point
  70  *
  71  */
  72 class TrajectoryPoints : public std::list<Point*> {
  73 public:
  74     ~TrajectoryPoints() {
  75         for (iterator wp = begin(); wp != end(); wp++) {
  76             delete(*wp);
  77         }
  78         clear();
  79     }
  80 };
  81
  82
  83 /**
  84  * Abstract class for a segment of the trajectory.
  85  *
  86  */
  87 class TrajectorySegment {
  88 protected:
  89     /** Time in the beginning and at the end of the segment. The time
  90      * is relative to the start of the whole trajectory. Units are seconds. */
  91     double t1, t2;
  92
  93 public:
  94     // speeds
  95     double maxv;                /**< Maximum speed of the segment. Determined by constraints of the whole trajectory. */
  96     double acc;                 /**< Acceleration at this segment [\f$\mathrm m\cdot \mathrm s^{-2}\f$] */
  97     double v1, v2;              ///< speed in the beginning and at the end of the segment
  98
  99     virtual void setMaxV(const TrajectoryConstraints &constr) = 0;
 100     virtual double getMaxAcc(const TrajectoryConstraints &constr) const { return constr.maxacc; };
 101     virtual bool isTurn() const { return false; };
 102     virtual bool isSpline() const { return false; };
 103     virtual double getDistance(double time) const = 0;
 104
 105     /// Helper values for Trajectory::corners2arcs()
 106     double s, r, alphahalf;
 107
 108     TrajectorySegment() : s(0), r(0), alphahalf(0){};
 109     virtual ~TrajectorySegment() {};
 110
 111     virtual double getLength() const = 0; /**< Length of the segment in meters. */
 112
 113     /** Unified length of the segment, used for calculations of
 114      * speed and accelerations. Normally, it equal to the real
 115      * length, buf for ::TrajectorySegmentTurn (whose length is zero)
 116      * it represents angle. */
 117     virtual double getUnifiedLength() const {
 118             return getLength();
 119     }
 120     /**
 121      * Returns the point of the segment, located at the specific @c
 122      * distance from the beginning or end.
 123      *
 124      * @param[in] distance Distance from the beginning (if positive) or end (if negative) of the segment in meters.
 125      * @param[out] p Pointer to the point to sore the result.
 126      */
 127     virtual void getPointAt(double distance, Point *p) = 0;
 128     /**
 129      * Shorten the segment by a specific length.
 130      *
 131      * @param[in] distance Distance from the end (if positive) of the segment, where should be the end of the segment.
 132      * @param[in] newEnd Point which will be set to the position of the new end.
 133      */
 134     virtual void shortenBy(double distance, Point *newEnd) = 0;
 135     /**
 136      * Splits the segment at the specified point. The result will be
 137      * two segments following the same trajectory as the original one.
 138      *
 139      * @param[in] distance Distance of the split from the beginning of the segment.
 140      * @param[in] newEnd Point which will be used as a middle point joining the two segments.
 141      *
 142      * @return Pointer to the newly created segment.
 143      */
 144     virtual TrajectorySegment* splitAt(double distance, Point *newEnd) = 0;
 145
 146     virtual TrajectorySegment* splitAtByTime(double time, Point *newEnd) = 0;
 147
 148     /**
 149      * Sets the starting time of the segment and calculates the ending
 150      * time according to this. Also acceleration is calculated from
 151      * these times and speed.
 152      *
 153      * @param time Starting time relative to the start of the whole
 154      * trajectory (in seconds).
 155      *
 156      * @return Ending time.
 157      */
 158     virtual double startAt(double time) {
 159         t1 = time;
 160         t2 = time + getUnifiedLength()/((v1+v2)/2.0);
 161         acc = (v2-v1)/(t2-t1);
 162         return t2;
 163     }
 164     /**
 165      * Return the reference position at some time.
 166      *
 167      * @param[in] time Time (within this segment time range).
 168      * @param[out] rp Reference position.
 169      */
 170     virtual void getRefPos(double time, Pos &rp) = 0;
 171     /**
 172      * Return the position in time of this segment relative to the
 173      * specified time.
 174      *
 175      * @param t Time
 176      *
 177      * @return
 178      * 0 if time @c t is within this segment,
 179      * -1 if time @c t is before this segment,
 180      * +1 if time @c t is after this segment.
 181      */
 182     int containsTime(double t) {
 183         if (t >= t1 && t <= t2)
 184             return 0;
 185         else if (t >= t2)
 186             return 1;
 187         else
 188             return -1;
 189     }
 190     double getT1() { return t1; } /**< Returns @c t1 */
 191     double getT2() { return t2; } /**< Returns @c t2 */
 192 #ifdef MATLAB_MEX_FILE
 193     virtual void plot(const char *style) = 0;
 194     virtual void plotSpeed(const char *style) {
 195         char cmd[300];
 196         sprintf(cmd, "plot([%g %g], [%g %g], '%s')",
 197                 t1, t2, v1, v2, style);
 198         mexEvalString(cmd);
 199
 200     }
 201 #endif
 202 };
 203
 204
 205 /**
 206  * Represents the whole trajectory. When a new trajectory is created
 207  * (by Trajectory() constructor), addPoint() should be used to add points,
 208  * the trajectory should go through. After all points are added,
 209  * a call to prepare() uses these points to construct optimized
 210  * trajectory and find the speeds along the trajectory. After the
 211  * trajectory is prepared, you can call getRefPos(), to get the
 212  * reference position at a specific time.
 213  *
 214  */
 215 class Trajectory : public std::list<TrajectorySegment*> {
 216     Pos initPos;
 217     iterator currentSeg;
 218     TrajectoryPoints wayPoints; /**< Points, this trajectory is composed from. */
 219     Point *initialPoint, *finalPoint;
 220     bool prepared;
 221     bool contains_inertial;
 222
 223     void deleteSegments() {
 224         for (iterator seg = begin(); seg != end(); seg++) {
 225             delete(*seg);
 226         }
 227         clear();
 228     }
 229
 230     bool splitSegment(iterator &seg, double distance);
 231     bool splitSegmentByTime(iterator &seg, double time);
 232     bool points2segments();
 233 //    void corners2arcs(); // not used
 234     void corners2splines();
 235     void addTurns();
 236     void calcSpeeds();
 237     void addInertialPoint();
 238     void log(const char* str);
 239
 240     void getSegRefPos(TrajectorySegment *seg, double time, Pos &rp);
 241
 242 public:
 243     TrajectoryConstraints constr;
 244     /** Desired heading of the robot at the end of the trajectory. */
 245     struct final_heading finalHeading;
 246     bool backward;
 247
 248     /**
 249      * Trajectory constructor.
 250      *
 251      * @param tc Trajectory constraints
 252      * @param _backward Whether the trajectory should be followed in backward direction.
 253      */
 254     Trajectory(TrajectoryConstraints tc, bool _backward = false) :
 255         wayPoints(), initialPoint(0), finalPoint(0),
 256         prepared(false), constr(tc), backward(_backward) {
 257                 finalHeading.turn_type = FH_DONT_TURN;
 258         };
 259
 260     ~Trajectory() {
 261         deleteSegments();
 262     }
 263
 264     /**
 265      * Adds a new point to the trajectory.
 266      *
 267      * @param x
 268      * @param y
 269      */
 270     void addPoint(double x, double y) {
 271         finalPoint = new Point(x,y);
 272         wayPoints.push_back(finalPoint);
 273     }
 274     void addTurn(double angle);
 275
 276     double calcLength();        /**< Returns the length of the whole trajectory. */
 277
 278     bool appendTrajectory(Trajectory &traj, double time);
 279     bool prepare(Pos _initPos);
 280     bool getRefPos(double time, Pos &rp);
 281 #ifdef MATLAB_MEX_FILE
 282     void plot(const char *style);
 283     void plotSpeed(const char *style);
 284 #endif
 285 #ifdef MOTION_LOG
 286     void logTraj(double start_time);
 287 #endif
 288
 289 };
 290
 291 namespace Segment {
 292         class Line : public TrajectorySegment {
 293                 Point *p1, *p2;
 294                 double angle;               // uhle v globalnich souradnicich
 295                 double length;              // delka segmentu
 296                 double cosphi, sinphi;
 297         public:
 298                 Line(Point *_p1, Point *_p2);
 299                 virtual void setMaxV(const TrajectoryConstraints &constr);
 300                 virtual double getLength() const {return length;}
 301                 virtual double getDistance(double time) const;
 302                 double getAngle() {return angle;}
 303                 virtual void getPointAt(double distance, Point *p);
 304                 virtual void shortenBy(double distance, Point *newEnd);
 305                 virtual TrajectorySegment* splitAt(double distance, Point *newEnd);
 306                 virtual TrajectorySegment* splitAtByTime(double time, Point *newEnd);
 307                 virtual void getRefPos(double time, Pos &rp);
 308 #ifdef MATLAB_MEX_FILE
 309                 virtual void plot(const char *style);
 310 #endif
 311         };
 312
 313         class Turn : public TrajectorySegment {
 314                 Point *center;
 315                 double turnBy;        /// turn by this angle [rad]
 316                 double startHeading;      /// turning starts at this heading [rad]
 317         public:
 318                 Turn(Point *_center, double _startHeading, double _turnBy);
 319                 virtual void setMaxV(const TrajectoryConstraints &constr);
 320                 virtual double getMaxAcc(const TrajectoryConstraints &constr) const {return constr.maxangacc;}
 321                 virtual bool isTurn() const { return true; };
 322                 virtual double getLength() const {return 0;}
 323                 virtual double getDistance(double time) const;
 324                 virtual double getUnifiedLength() const {return fabs(turnBy);}
 325                 virtual void getPointAt(double distance, Point *p);
 326                 virtual void shortenBy(double, Point *) {}
 327                 virtual TrajectorySegment* splitAt(double distance, Point *newEnd);
 328                 virtual TrajectorySegment* splitAtByTime(double time, Point *newEnd);
 329                 virtual void getRefPos(double time, Pos &rp);
 330 #ifdef MATLAB_MEX_FILE
 331                 virtual void plot(const char *style) {};
 332 #endif
 333         };
 334
 335         /**
 336          * Spline shaped segment.
 337          *
 338          * The spline is represented by x-axis polynom and y-axis polynom. Both have 5-th order.
 339          * P_x(t) = A_x*t^5 + B_x*t^4 + C_x*t^3 + D_x*t^2 + E_x*t + F_x
 340          * P_y(t) = A_y*t^5 + B_y*t^4 + C_y*t^3 + D_y*t^2 + E_y*t + F_y
 341          * The parameter t goes from param0 to param1 (initially 0 and 1)
 342          * We pressume that the parameter dt/ds = const (reasonable simplification)
 343          *
 344          * TODO: comment
 345          */
 346         class Spline : public TrajectorySegment {
 347                 Point *p1, *p2;                 // first and final point
 348                 double Ax, Bx, Cx, Dx, Ex, Fx;  // constants representing x-axis polynom
 349                 double Ay, By, Cy, Dy, Ey, Fy;  // constants representing y-axis polynom
 350                 double length;                  // segment length
 351                 Point *corner;                  // Corner point this spline is substituted for
 352                 double distance;                // distance form p1 to the corner (or p2 to the corner as well)
 353                 double gamma;           // central angle of the corner
 354                 double m;                       // size of starting and final speed vector (parameter of spline shaping)
 355                 double param0, param1;  // parameter value at the begining and end of the segment (initially 0 and 1)
 356                 double vc;          // speed in the center (lowest - according to clothoid approximation)
 357                 double tc;          // time in the center
 358                 double acc1;    // acceleration in the first half (typically negative)
 359                 double acc2;    // acceleration in the second half (typically positive)
 360         public:
 361                 Spline(Point *_p1, Point *_p2, Point *_corner);
 362                 virtual bool isSpline() const { return true; }
 363                 virtual void setMaxV(const TrajectoryConstraints &constr);
 364                 virtual double getLength() const { return length; }
 365                 virtual double getDistance(double time) const;
 366                 virtual void getPointAt(double distance, Point *p);
 367                 virtual void shortenBy(double distance, Point *newEnd);
 368                 virtual TrajectorySegment* splitAtByTime(double time, Point *newEnd);
 369                 virtual TrajectorySegment* splitAt(double distance, Point *newEnd);
 370                 virtual void getRefPos(double time, Pos &rp);
 371                 virtual double startAt(double time);
 372 #ifdef MATLAB_MEX_FILE
 373                 virtual void plot(const char *style);
 374                 virtual void plotSpeed(const char *style);
 375 #endif
 376         private:
 377                 double dist2param(double distance);
 378                 void getPointAtParam(double par, Point *p);
 379                 double getAngleAt(double distance);
 380                 double getRadiusAtParam(double par);
 381                 double getRadiusAt(double distance);
 382         };
 383
 384         /**
 385          * Arc shaped segment.
 386          */
 387         class Arc : public TrajectorySegment
 388         {
 389                 Point *p1, *p2;
 390                 double angle;               // vrcholovy uhel [-pi,+pi]
 391                 double startAngle;          // uhel od stredu k p1
 392                 double radius;              // polomer
 393                 double length;              // delka segmentu
 394                 Point center;
 395         public:
 396                 Arc(Point *_p1, Point *_p2, double _radius);
 397                 virtual void setMaxV(const TrajectoryConstraints &constr);
 398                 virtual double getLength() const {return length;}
 399                 virtual double getDistance(double time) const;
 400                 virtual void getPointAt(double distance, Point *p);
 401                 virtual void shortenBy(double distance, Point *newEnd);
 402                 virtual TrajectorySegment* splitAt(double distance, Point *newEnd);
 403                 virtual TrajectorySegment* splitAtByTime(double time, Point *newEnd);
 404                 virtual void getRefPos(double time, Pos &rp);
 405 #ifdef MATLAB_MEX_FILE
 406                 virtual void plot(const char *style);
 407 #endif
 408         };
 409
 410 };
 411
 412
 413 //@}
 414
 415 #endif