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