The dubins-path-pp from mehhdiii

dubins-path-pp's Introduction

Hey there fellow engineer 👋

☕ always up for philosophy-101 discussions over a cup of tea/coffee

and also,

🔭 I work with web technologies
🌱 trying to write more poetic code
👯 always looking for book recommendations
📫 reach me at: [email protected]
📖 know me better here: https://www.goodreads.com/user/show/137288599-mehdi

dubins-path-pp's People

Stargazers

Watchers

dubins-path-pp's Issues

Two question

The speed direction does not match the path.
All paths are not exhausted.
consider about the following file
%starting Pose
xs = 0; ys = 0; phis=deg2rad(90);
%final Pose
xf = 10; yf = 10; phif=deg2rad(70);
%curvature constraints:半径大小
rhos = 3; rhof = 1;
Mode = 3;%1、L+L 2、L+R 3、R+R 4、R+L
%center of curvature circles: 左转圆和右转圆
xcs_L = xs - rhoscos(phis - pi/2);
ycs_L = ys - rhossin(phis - pi/2);
xcf_L = xf - rhofcos(phif - pi/2);
ycf_L = xf - rhofsin(phif - pi/2);
xcs_R = xs + rhoscos(phis - pi/2);
ycs_R = ys + rhossin(phis - pi/2);
xcf_R = xf + rhofcos(phif - pi/2);
ycf_R = xf + rhofsin(phif - pi/2);
%%
switch Mode
case 1%L+L
%tangent entry (E) and exit (X)
c = sqrt((xcs_L - xcf_L)^2 + (ycs_L-ycf_L)^2);
alpha = asin((rhof-rhos)/c);
beta = atan2(ycf_L-ycs_L, xcf_L - xcs_L);
%E
% phi = alpha+beta +pi/2; % right turn
phi = beta-alpha +3pi/2; % left turn
xPX = xcs_L + rhoscos(phi);
yPX = ycs_L + rhossin(phi);
%X
%phi = alpha+beta +pi/2; %right turn
phi = beta-alpha +3pi/2; %left turn
xPN = xcf_L + rhofcos(phi);
yPN = ycf_L + rhofsin(phi);
xcs = xcs_L;ycs = ycs_L; xcf = xcf_L; ycf = ycf_L;
case 2%L+R
%tangent entry (E) and exit (X)
c = sqrt((xcs_L - xcf_R)^2 + (ycs_L-ycf_R)^2);
alpha = asin((rhof-rhos)/c);
beta = atan2(ycf_R-ycs_L, xcf_R - xcs_L);
%E
% phi = alpha+beta +pi/2; % right turn
phi = beta-alpha +3pi/2; % left turn
xPX = xcs_L + rhoscos(phi);
yPX = ycs_L + rhossin(phi);
%X
phi = alpha+beta +pi/2; %right turn
%phi = beta-alpha +3pi/2; %left turn
xPN = xcf_R + rhofcos(phi);
yPN = ycf_R + rhofsin(phi);
xcs = xcs_L;ycs = ycs_L; xcf = xcf_R; ycf = ycf_R;
case 3% R+R
%tangent entry (E) and exit (X)
c = sqrt((xcs_R - xcf_R)^2 + (ycs_R-ycf_R)^2);
alpha = asin((rhof-rhos)/c);
beta = atan2(ycf_R-ycs_R, xcf_R - xcs_R);
%E
phi = alpha+beta +pi/2; % right turn
%phi = beta-alpha +3pi/2; % left turn
xPX = xcs_R + rhoscos(phi);
yPX = ycs_R + rhossin(phi);
%X
phi = alpha+beta +pi/2; %right turn
%phi = beta-alpha +3pi/2; %left turn
xPN = xcf_R + rhofcos(phi);
yPN = ycf_R + rhofsin(phi);
xcs = xcs_R;ycs = ycs_R; xcf = xcf_R; ycf = ycf_R;
otherwise%R+L
%tangent entry (E) and exit (X)
c = sqrt((xcs_R - xcf_L)^2 + (ycs_R-ycf_L)^2);
alpha = asin((rhof-rhos)/c);
beta = atan2(ycf_L-ycs_R, xcf_L - xcs_R);
%E
phi = alpha+beta +pi/2; % right turn
%phi = beta-alpha +3pi/2; % left turn
xPX = xcs_R + rhoscos(phi);
yPX = ycs_R + rhossin(phi);
%X
%phi = alpha+beta +pi/2; %right turn
phi = beta-alpha +3pi/2; %left turn
xPN = xcf_L + rhofcos(phi);
yPN = ycf_L + rhofsin(phi);
xcs = xcs_R;ycs = ycs_R; xcf = xcf_L; ycf = ycf_L;
end
%%
%%%%%绘图
close all;
figure()
hold on
%plot circles:
centers = [xcs ycs; xcf ycf]; radii = [rhos; rhof];
viscircles(centers,radii)%画多个圆
%plot poses:
scatter(xs, ys,45, '','r','LineWidth',5);
scatter(xf, yf,45, '','r','LineWidth',5);
%plot center line
xc = [xcs xcf];
yc = [ycs ycf];
plot(xc, yc, '--')
xT = [xPX xPN]; yT = [yPX yPN];
plot(xT, yT, 'k','lineWidth', 3)
hold off
daspect([1,1,1])

Recommend Projects