andrewwalker / dubins-curves Goto Github PK

Sampling is done along the Dubins path between a start point (16.2953, 0.12524, 0.575959) and and endpoint (17.2329, 2.0764, 2.28307). As the samples get closer to the endpoint, the distance between the current sample to the endpoint jumps at some point. Below is the test code, followed by the test output.

#include "dubins.h"
#include <stdio.h>

double start[] = { 16.2953, 0.12524, 0.575959 };
double end[] = { 17.2329, 2.0764, 2.28307 };

int printConfiguration(double q[3], double x, void* user_data) {
    DubinsPath path;
    dubins_shortest_path(&path, q, end, 1.0);

    printf("%f, %f, %f, %f, %f\n", q[0], q[1], q[2], x, dubins_path_length(&path));
    return 0;
}

int main()
{
    DubinsPath path;
    dubins_shortest_path(&path, start, end, 1.0);

    printf("#x,y,theta,t\n");
    dubins_path_sample_many(&path,  0.1, printConfiguration, NULL);

    return 0;
}

Test output:

0: 16.295300, 0.125240, 0.575959, 0.000000, 2.565464
1: 16.379776, 0.178753, 0.563946, 0.100000, 2.465464
2: 16.464292, 0.232206, 0.563946, 0.200000, 2.365464
3: 16.548807, 0.285658, 0.563946, 0.300000, 2.265464
4: 16.633322, 0.339111, 0.563946, 0.400000, 2.165464
5: 16.717837, 0.392564, 0.563946, 0.500000, 2.065464
6: 16.802353, 0.446016, 0.563946, 0.600000, 1.965464
7: 16.886868, 0.499469, 0.563946, 0.700000, 1.865464
8: 16.971383, 0.552921, 0.563946, 0.800000, 1.765464
9: 17.055107, 0.607576, 0.617606, 0.900000, 7.569978
10: 17.133605, 0.669461, 0.717606, 1.000000, 7.848649
11: 17.205533, 0.738874, 0.817606, 1.100000, 7.748649
12: 17.270171, 0.815121, 0.917606, 1.200000, 7.648649
13: 17.326875, 0.897439, 1.017606, 1.300000, 7.548649
14: 17.375077, 0.985008, 1.117606, 1.400000, 7.448649
15: 17.414296, 1.076951, 1.217606, 1.500000, 7.348649
16: 17.444140, 1.172350, 1.317606, 1.600000, 7.248649
17: 17.464311, 1.270252, 1.417606, 1.700000, 7.148649
18: 17.474608, 1.369678, 1.517606, 1.800000, 7.048649
19: 17.474927, 1.469636, 1.617606, 1.900000, 6.948649
20: 17.465265, 1.569126, 1.717606, 2.000000, 6.848649
21: 17.445719, 1.667155, 1.817606, 2.100000, 6.748649
22: 17.416484, 1.762743, 1.917606, 2.200000, 6.648649
23: 17.377852, 1.854934, 2.017606, 2.300000, 6.548649
24: 17.330210, 1.942808, 2.117606, 2.400000, 6.448649
25: 17.274033, 2.025487, 2.217606, 2.500000, 6.348649

The distance spikes from sample 8 to 9, not sure if this can be considered expected behavior.

Hello,

I am working on dubins path, and your code works great!!!.

I wonder if your code includes CCC configuration? For any set of initial and final points, I could get only CSC configuration. If the code does include CCC configuration, please suggest some set of initial and final conditions, which results in this.

Thanks

Thanks for your code,It really help me a lot,But I can't find the turning radius value in your code.Could you tell me that?I really appreciate your help!

First of all, thank you for your source code, it is a good work! However, I have been testing it and I think I have found some unexpected result. I tried to find the shortest path for the inputs:

p1: (0, 0, pi/2)
p2: (4, 0, -pi/2)
Minimum turning radius(rho): 3

I think the shortest path for this configuration it is:

But I'm getting this one:

The code is retrieving a RLR trajectory instead of a LRL. May you tell me if it is a bug or if there is another thing I'm not taking into account?

Hello,

The code works great!!

Please include a file explaining the Algorithm in generating Dubin's path.

Thanks
Avinash

Hope for 3d dubins curves.
Input:
Start Point(x, y, z, phi, gamma)
End Point(x', y', z', phi', gamma')
radius

Output:
Path and length

The Matlab wrapper found here: http://au.mathworks.com/matlabcentral/fileexchange/40655-dubins-curve-mex uses a old version of this code and does not compile on newer versions of Matlab.

I have published a updated wrapper here: https://gist.github.com/Lauszus/e150347185f6012735b776d06aa6ca48 which uses the code from this repository directly, so any future releases should work as well as long as the API does not change.

I have also included a small example which shows how to use this code to generate paths from some waypoints.

Making this change will support more effective interpolation strategies (for CSC curves, only the endpoints of the straight segment need to be considered).

Hello, I have a question about the function dubins_LRL, I read the paper "Classification of the Dubins set", and in the paper p= mode2pi(acos(tmp0)), but in your implementation p = mode2pi(2*pi - acos(tmp0)), and out[2] is also different. Could you please tell me why your implementation is different from the paper? Thank you!

int dubins_LRL(DubinsIntermediateResults* in, double out[3])
{
double tmp0 = (6. - in->d_sq + 2in->c_ab + 2in->d*(in->sb - in->sa)) / 8.;
double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
if( fabs(tmp0) <= 1) {
double p = mod2pi( 2*M_PI - acos( tmp0) );
double t = mod2pi(-in->alpha - phi + p/2.);
out[0] = t;
out[1] = p;
out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
return EDUBOK;
}
return EDUBNOPATH;
}