vehicle_platform/dubins.cc

   1 /*
   2  * Copyright (c) 2008-2018, Andrew Walker
   3  *
   4  * Permission is hereby granted, free of charge, to any person obtaining a copy
   5  * of this software and associated documentation files (the "Software"), to deal
   6  * in the Software without restriction, including without limitation the rights
   7  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
   8  * copies of the Software, and to permit persons to whom the Software is
   9  * furnished to do so, subject to the following conditions:
  10  *
  11  * The above copyright notice and this permission notice shall be included in
  12  * all copies or substantial portions of the Software.
  13  *
  14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  20  * THE SOFTWARE.
  21  */
  22 #ifdef WIN32
  23 #define _USE_MATH_DEFINES
  24 #endif
  25 #include <math.h>
  26 #include "dubins.h"
  27
  28 #define EPSILON (10e-10)
  29
  30 typedef enum
  31 {
  32     L_SEG = 0,
  33     S_SEG = 1,
  34     R_SEG = 2
  35 } SegmentType;
  36
  37 /* The segment types for each of the Path types */
  38 const SegmentType DIRDATA[][3] = {
  39     { L_SEG, S_SEG, L_SEG },
  40     { L_SEG, S_SEG, R_SEG },
  41     { R_SEG, S_SEG, L_SEG },
  42     { R_SEG, S_SEG, R_SEG },
  43     { R_SEG, L_SEG, R_SEG },
  44     { L_SEG, R_SEG, L_SEG }
  45 };
  46
  47 typedef struct
  48 {
  49     double alpha;
  50     double beta;
  51     double d;
  52     double sa;
  53     double sb;
  54     double ca;
  55     double cb;
  56     double c_ab;
  57     double d_sq;
  58 } DubinsIntermediateResults;
  59
  60
  61 int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3]);
  62 int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho);
  63
  64 /**
  65  * Floating point modulus suitable for rings
  66  *
  67  * fmod doesn't behave correctly for angular quantities, this function does
  68  */
  69 double fmodr( double x, double y)
  70 {
  71     return x - y*floor(x/y);
  72 }
  73
  74 double mod2pi( double theta )
  75 {
  76     return fmodr( theta, 2 * M_PI );
  77 }
  78
  79 int dubins_shortest_path(DubinsPath* path, double q0[3], double q1[3], double rho)
  80 {
  81     int i, errcode;
  82     DubinsIntermediateResults in;
  83     double params[3];
  84     double cost;
  85     double best_cost = INFINITY;
  86     int best_word = -1;
  87     errcode = dubins_intermediate_results(&in, q0, q1, rho);
  88     if(errcode != EDUBOK) {
  89         return errcode;
  90     }
  91
  92
  93     path->qi[0] = q0[0];
  94     path->qi[1] = q0[1];
  95     path->qi[2] = q0[2];
  96     path->rho = rho;
  97
  98     for( i = 0; i < 6; i++ ) {
  99         DubinsPathType pathType = (DubinsPathType)i;
 100         errcode = dubins_word(&in, pathType, params);
 101         if(errcode == EDUBOK) {
 102             cost = params[0] + params[1] + params[2];
 103             if(cost < best_cost) {
 104                 best_word = i;
 105                 best_cost = cost;
 106                 path->param[0] = params[0];
 107                 path->param[1] = params[1];
 108                 path->param[2] = params[2];
 109                 path->type = pathType;
 110             }
 111         }
 112     }
 113     if(best_word == -1) {
 114         return EDUBNOPATH;
 115     }
 116     return EDUBOK;
 117 }
 118
 119 int dubins_path(DubinsPath* path, double q0[3], double q1[3], double rho, DubinsPathType pathType)
 120 {
 121     int errcode;
 122     DubinsIntermediateResults in;
 123     errcode = dubins_intermediate_results(&in, q0, q1, rho);
 124     if(errcode == EDUBOK) {
 125         double params[3];
 126         errcode = dubins_word(&in, pathType, params);
 127         if(errcode == EDUBOK) {
 128             path->param[0] = params[0];
 129             path->param[1] = params[1];
 130             path->param[2] = params[2];
 131             path->qi[0] = q0[0];
 132             path->qi[1] = q0[1];
 133             path->qi[2] = q0[2];
 134             path->rho = rho;
 135             path->type = pathType;
 136         }
 137     }
 138     return errcode;
 139 }
 140
 141 double dubins_path_length( DubinsPath* path )
 142 {
 143     double length = 0.;
 144     length += path->param[0];
 145     length += path->param[1];
 146     length += path->param[2];
 147     length = length * path->rho;
 148     return length;
 149 }
 150
 151 double dubins_segment_length( DubinsPath* path, int i )
 152 {
 153     if( (i < 0) || (i > 2) )
 154     {
 155         return INFINITY;
 156     }
 157     return path->param[i] * path->rho;
 158 }
 159
 160 double dubins_segment_length_normalized( DubinsPath* path, int i )
 161 {
 162     if( (i < 0) || (i > 2) )
 163     {
 164         return INFINITY;
 165     }
 166     return path->param[i];
 167 }
 168
 169 DubinsPathType dubins_path_type( DubinsPath* path )
 170 {
 171     return path->type;
 172 }
 173
 174 void dubins_segment( double t, double qi[3], double qt[3], SegmentType type)
 175 {
 176     double st = sin(qi[2]);
 177     double ct = cos(qi[2]);
 178     if( type == L_SEG ) {
 179         qt[0] = +sin(qi[2]+t) - st;
 180         qt[1] = -cos(qi[2]+t) + ct;
 181         qt[2] = t;
 182     }
 183     else if( type == R_SEG ) {
 184         qt[0] = -sin(qi[2]-t) + st;
 185         qt[1] = +cos(qi[2]-t) - ct;
 186         qt[2] = -t;
 187     }
 188     else if( type == S_SEG ) {
 189         qt[0] = ct * t;
 190         qt[1] = st * t;
 191         qt[2] = 0.0;
 192     }
 193     qt[0] += qi[0];
 194     qt[1] += qi[1];
 195     qt[2] += qi[2];
 196 }
 197
 198 int dubins_path_sample( DubinsPath* path, double t, double q[3] )
 199 {
 200     /* tprime is the normalised variant of the parameter t */
 201     double tprime = t / path->rho;
 202     double qi[3]; /* The translated initial configuration */
 203     double q1[3]; /* end-of segment 1 */
 204     double q2[3]; /* end-of segment 2 */
 205     const SegmentType* types = DIRDATA[path->type];
 206     double p1, p2;
 207
 208     if( t < 0 || t > dubins_path_length(path) ) {
 209         return EDUBPARAM;
 210     }
 211
 212     /* initial configuration */
 213     qi[0] = 0.0;
 214     qi[1] = 0.0;
 215     qi[2] = path->qi[2];
 216
 217     /* generate the target configuration */
 218     p1 = path->param[0];
 219     p2 = path->param[1];
 220     dubins_segment( p1,      qi,    q1, types[0] );
 221     dubins_segment( p2,      q1,    q2, types[1] );
 222     if( tprime < p1 ) {
 223         dubins_segment( tprime, qi, q, types[0] );
 224     }
 225     else if( tprime < (p1+p2) ) {
 226         dubins_segment( tprime-p1, q1, q,  types[1] );
 227     }
 228     else {
 229         dubins_segment( tprime-p1-p2, q2, q,  types[2] );
 230     }
 231
 232     /* scale the target configuration, translate back to the original starting point */
 233     q[0] = q[0] * path->rho + path->qi[0];
 234     q[1] = q[1] * path->rho + path->qi[1];
 235     q[2] = mod2pi(q[2]);
 236
 237     return EDUBOK;
 238 }
 239
 240 int dubins_path_sample_many(DubinsPath* path, double stepSize,
 241                             DubinsPathSamplingCallback cb, void* user_data)
 242 {
 243     int retcode;
 244     double q[3];
 245     double x = 0.0;
 246     double length = dubins_path_length(path);
 247     while( x <  length ) {
 248         dubins_path_sample( path, x, q );
 249         retcode = cb(q, x, user_data);
 250         if( retcode != 0 ) {
 251             return retcode;
 252         }
 253         x += stepSize;
 254     }
 255     return 0;
 256 }
 257
 258 int dubins_path_endpoint( DubinsPath* path, double q[3] )
 259 {
 260     return dubins_path_sample( path, dubins_path_length(path) - EPSILON, q );
 261 }
 262
 263 int dubins_extract_subpath( DubinsPath* path, double t, DubinsPath* newpath )
 264 {
 265     /* calculate the true parameter */
 266     double tprime = t / path->rho;
 267
 268     if((t < 0) || (t > dubins_path_length(path)))
 269     {
 270         return EDUBPARAM;
 271     }
 272
 273     /* copy most of the data */
 274     newpath->qi[0] = path->qi[0];
 275     newpath->qi[1] = path->qi[1];
 276     newpath->qi[2] = path->qi[2];
 277     newpath->rho   = path->rho;
 278     newpath->type  = path->type;
 279
 280     /* fix the parameters */
 281     newpath->param[0] = fmin( path->param[0], tprime );
 282     newpath->param[1] = fmin( path->param[1], tprime - newpath->param[0]);
 283     newpath->param[2] = fmin( path->param[2], tprime - newpath->param[0] - newpath->param[1]);
 284     return 0;
 285 }
 286
 287 int dubins_intermediate_results(DubinsIntermediateResults* in, double q0[3], double q1[3], double rho)
 288 {
 289     double dx, dy, D, d, theta, alpha, beta;
 290     if( rho <= 0.0 ) {
 291         return EDUBBADRHO;
 292     }
 293
 294     dx = q1[0] - q0[0];
 295     dy = q1[1] - q0[1];
 296     D = sqrt( dx * dx + dy * dy );
 297     d = D / rho;
 298     theta = 0;
 299
 300     /* test required to prevent domain errors if dx=0 and dy=0 */
 301     if(d > 0) {
 302         theta = mod2pi(atan2( dy, dx ));
 303     }
 304     alpha = mod2pi(q0[2] - theta);
 305     beta  = mod2pi(q1[2] - theta);
 306
 307     in->alpha = alpha;
 308     in->beta  = beta;
 309     in->d     = d;
 310     in->sa    = sin(alpha);
 311     in->sb    = sin(beta);
 312     in->ca    = cos(alpha);
 313     in->cb    = cos(beta);
 314     in->c_ab  = cos(alpha - beta);
 315     in->d_sq  = d * d;
 316
 317     return EDUBOK;
 318 }
 319
 320 int dubins_LSL(DubinsIntermediateResults* in, double out[3])
 321 {
 322     double tmp0, tmp1, p_sq;
 323
 324     tmp0 = in->d + in->sa - in->sb;
 325     p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));
 326
 327     if(p_sq >= 0) {
 328         tmp1 = atan2( (in->cb - in->ca), tmp0 );
 329         out[0] = mod2pi(tmp1 - in->alpha);
 330         out[1] = sqrt(p_sq);
 331         out[2] = mod2pi(in->beta - tmp1);
 332         return EDUBOK;
 333     }
 334     return EDUBNOPATH;
 335 }
 336
 337
 338 int dubins_RSR(DubinsIntermediateResults* in, double out[3])
 339 {
 340     double tmp0 = in->d - in->sa + in->sb;
 341     double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
 342     if( p_sq >= 0 ) {
 343         double tmp1 = atan2( (in->ca - in->cb), tmp0 );
 344         out[0] = mod2pi(in->alpha - tmp1);
 345         out[1] = sqrt(p_sq);
 346         out[2] = mod2pi(tmp1 -in->beta);
 347         return EDUBOK;
 348     }
 349     return EDUBNOPATH;
 350 }
 351
 352 int dubins_LSR(DubinsIntermediateResults* in, double out[3])
 353 {
 354     double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
 355     if( p_sq >= 0 ) {
 356         double p    = sqrt(p_sq);
 357         double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
 358         out[0] = mod2pi(tmp0 - in->alpha);
 359         out[1] = p;
 360         out[2] = mod2pi(tmp0 - mod2pi(in->beta));
 361         return EDUBOK;
 362     }
 363     return EDUBNOPATH;
 364 }
 365
 366 int dubins_RSL(DubinsIntermediateResults* in, double out[3])
 367 {
 368     double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
 369     if( p_sq >= 0 ) {
 370         double p    = sqrt(p_sq);
 371         double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
 372         out[0] = mod2pi(in->alpha - tmp0);
 373         out[1] = p;
 374         out[2] = mod2pi(in->beta - tmp0);
 375         return EDUBOK;
 376     }
 377     return EDUBNOPATH;
 378 }
 379
 380 int dubins_RLR(DubinsIntermediateResults* in, double out[3])
 381 {
 382     double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
 383     double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
 384     if( fabs(tmp0) <= 1) {
 385         double p = mod2pi((2*M_PI) - acos(tmp0) );
 386         double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
 387         out[0] = t;
 388         out[1] = p;
 389         out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
 390         return EDUBOK;
 391     }
 392     return EDUBNOPATH;
 393 }
 394
 395 int dubins_LRL(DubinsIntermediateResults* in, double out[3])
 396 {
 397     double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
 398     double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
 399     if( fabs(tmp0) <= 1) {
 400         double p = mod2pi( 2*M_PI - acos( tmp0) );
 401         double t = mod2pi(-in->alpha - phi + p/2.);
 402         out[0] = t;
 403         out[1] = p;
 404         out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
 405         return EDUBOK;
 406     }
 407     return EDUBNOPATH;
 408 }
 409
 410 int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, double out[3])
 411 {
 412     int result;
 413     switch(pathType)
 414     {
 415     case LSL:
 416         result = dubins_LSL(in, out);
 417         break;
 418     case RSL:
 419         result = dubins_RSL(in, out);
 420         break;
 421     case LSR:
 422         result = dubins_LSR(in, out);
 423         break;
 424     case RSR:
 425         result = dubins_RSR(in, out);
 426         break;
 427     case LRL:
 428         result = dubins_LRL(in, out);
 429         break;
 430     case RLR:
 431         result = dubins_RLR(in, out);
 432         break;
 433     default:
 434         result = EDUBNOPATH;
 435     }
 436     return result;
 437 }
 438
 439