diff --git a/minimum_snap_trajectory_generation/README.md b/minimum_snap_trajectory_generation/README.md new file mode 100644 index 0000000..cddf954 --- /dev/null +++ b/minimum_snap_trajectory_generation/README.md @@ -0,0 +1,19 @@ +# minimum snap trajectory planning in MATLAB +This repository contains sample code in MATLAB for minimum snap trajectory planning described in http://blog.csdn.net/q597967420/article/details/73647190. +This README provides a brief overview of our trajectory generation utilities with some examples. + +## Required +1. MATLAB(R2013a is tested) + +## How to run +There are 4 demo codes in 2D space, you can directly run these and see results. + + - demo1: minimum snap trajectory planning with waypoints **strong constrants**(equality constraints). + - demo2: minimum snap trajectory planning with **corridor constraints**(iequality constraints). + - demo3: minimum snap trajectory planning with waypoints strong constrants by **close form** solution. + - demo4: minimum snap trajectory planning with **guiding path** + +## Licence +The source code is released under GPLv3 license. + +Any problem, please contact maoshuyuan123@gmail.com \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/a.fig b/minimum_snap_trajectory_generation/a.fig new file mode 100644 index 0000000..db950cd Binary files /dev/null and b/minimum_snap_trajectory_generation/a.fig differ diff --git a/minimum_snap_trajectory_generation/arrangeT.m b/minimum_snap_trajectory_generation/arrangeT.m new file mode 100644 index 0000000..bbc5b30 --- /dev/null +++ b/minimum_snap_trajectory_generation/arrangeT.m @@ -0,0 +1,6 @@ +function ts = arrangeT(waypts,T) + x = waypts(:,2:end) - waypts(:,1:end-1); + dist = sum(x.^2,1).^0.5; + k = T/sum(dist); + ts = [0 cumsum(dist*k)]; +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/calc_tvec.m b/minimum_snap_trajectory_generation/calc_tvec.m new file mode 100644 index 0000000..98bdfb4 --- /dev/null +++ b/minimum_snap_trajectory_generation/calc_tvec.m @@ -0,0 +1,7 @@ +% r=0:pos 1:vel 2:acc 3:jerk +function tvec = calc_tvec(t,n_order,r) + tvec = zeros(1,n_order+1); + for i=r+1:n_order+1 + tvec(i) = prod(i-r:i-1)*t^(i-r-1); + end +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/computeQ.m b/minimum_snap_trajectory_generation/computeQ.m new file mode 100644 index 0000000..2c234f7 --- /dev/null +++ b/minimum_snap_trajectory_generation/computeQ.m @@ -0,0 +1,19 @@ +% n:polynormial order +% r:derivertive order, 1:minimum vel 2:minimum acc 3:minimum jerk 4:minimum snap +% t1:start timestamp for polynormial +% t2:end timestap for polynormial +function Q = computeQ(n,r,t1,t2) +T = zeros((n-r)*2+1,1); +for i = 1:(n-r)*2+1 + T(i) = t2^i-t1^i; +end +Q = zeros(n); +for i = r+1:n+1 + for j = i:n+1 + k1 = i-r-1; + k2 = j-r-1; + k = k1+k2+1; + Q(i,j) = prod(k1+1:k1+r)*prod(k2+1:k2+r)/k*T(k); + Q(j,i) = Q(i,j); + end +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/demo1_minimum_snap_simple.m b/minimum_snap_trajectory_generation/demo1_minimum_snap_simple.m new file mode 100644 index 0000000..518c5f2 --- /dev/null +++ b/minimum_snap_trajectory_generation/demo1_minimum_snap_simple.m @@ -0,0 +1,110 @@ +function demo1_minimum_snap_simple() + clear,clc; + + %% condition + waypts = [0,0; + 1,2; + 2,-1; + 4,8; + 5,2]'; + v0 = [0,0]; + a0 = [0,0]; + v1 = [0,0]; + a1 = [0,0]; + T = 5; + ts = arrangeT(waypts,T); + n_order = 5; + + %% trajectory plan + polys_x = minimum_snap_single_axis_simple(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1)); + polys_y = minimum_snap_single_axis_simple(waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2)); + + %% result show + figure(1) + plot(waypts(1,:),waypts(2,:),'*r');hold on; + plot(waypts(1,:),waypts(2,:),'b--'); + title('minimum snap trajectory'); + color = ['grc']; + for i=1:size(polys_x,2) + tt = ts(i):0.01:ts(i+1); + xx = polys_vals(polys_x,ts,tt,0); + yy = polys_vals(polys_y,ts,tt,0); + plot(xx,yy,color(mod(i,3)+1)); + end + + figure(2) + vxx = polys_vals(polys_x,ts,tt,1); + axx = polys_vals(polys_x,ts,tt,2); + jxx = polys_vals(polys_x,ts,tt,3); + vyy = polys_vals(polys_y,ts,tt,1); + ayy = polys_vals(polys_y,ts,tt,2); + jyy = polys_vals(polys_y,ts,tt,3); + + subplot(421),plot(tt,xx);title('x position'); + subplot(422),plot(tt,yy);title('y position'); + subplot(423),plot(tt,vxx);title('x velocity'); + subplot(424),plot(tt,vyy);title('y velocity'); + subplot(425),plot(tt,axx);title('x acceleration'); + subplot(426),plot(tt,ayy);title('y acceleration'); + subplot(427),plot(tt,jxx);title('x jerk'); + subplot(428),plot(tt,jyy);title('y jerk'); +end + + +function polys = minimum_snap_single_axis_simple(waypts,ts,n_order,v0,a0,ve,ae) +p0 = waypts(1); +pe = waypts(end); + +n_poly = length(waypts)-1; +n_coef = n_order+1; + +% compute Q +Q_all = []; +for i=1:n_poly + Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1))); +end +b_all = zeros(size(Q_all,1),1); + +Aeq = zeros(4*n_poly+2,n_coef*n_poly); +beq = zeros(4*n_poly+2,1); + +% start/terminal pva constraints (6 equations) +Aeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0); + calc_tvec(ts(1),n_order,1); + calc_tvec(ts(1),n_order,2)]; +Aeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ... + [calc_tvec(ts(end),n_order,0); + calc_tvec(ts(end),n_order,1); + calc_tvec(ts(end),n_order,2)]; +beq(1:6,1) = [p0,v0,a0,pe,ve,ae]'; + +% mid p constraints (n_ploy-1 equations) +neq = 6; +for i=1:n_poly-1 + neq=neq+1; + Aeq(neq,n_coef*i+1:n_coef*(i+1)) = calc_tvec(ts(i+1),n_order,0); + beq(neq) = waypts(i+1); +end + +% continuous constraints ((n_poly-1)*3 equations) +for i=1:n_poly-1 + tvec_p = calc_tvec(ts(i+1),n_order,0); + tvec_v = calc_tvec(ts(i+1),n_order,1); + tvec_a = calc_tvec(ts(i+1),n_order,2); + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p]; + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v]; + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a]; +end + +Aieq = []; +bieq = []; + +p = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq); + +polys = reshape(p,n_coef,n_poly); + +end + diff --git a/minimum_snap_trajectory_generation/demo2_minimum_snap_corridor.m b/minimum_snap_trajectory_generation/demo2_minimum_snap_corridor.m new file mode 100644 index 0000000..0244d50 --- /dev/null +++ b/minimum_snap_trajectory_generation/demo2_minimum_snap_corridor.m @@ -0,0 +1,125 @@ +function demo2_minimum_snap_corridor() +clear,clc; + +%% condition +waypts = [0,0; + 1,2; + 2,0; + 4,5; + 5,2]'; +v0 = [0,0]; +a0 = [0,0]; +v1 = [0,0]; +a1 = [0,0]; +T = 5; +n_order = 5; + +%% sample mid points +r = 0.2; %% corridor r +step = r; +new_waypts = waypts(:,1); +for i=2:size(waypts,2) + x1 = waypts(1,i-1); + y1 = waypts(2,i-1); + x2 = waypts(1,i); + y2 = waypts(2,i); + n = ceil(hypot(x1-x2,y1-y2)/step)+1; + sample_pts = [linspace(x1,x2,n);linspace(y1,y2,n)]; + new_waypts = [new_waypts sample_pts(:,2:end)]; +end +ts = arrangeT(new_waypts,T); + +%% trajectory plan +polys_x = minimum_snap_single_axis_corridor(new_waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1),r); +polys_y = minimum_snap_single_axis_corridor(new_waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2),r); + +%% result show +figure(1) +% plot(waypts(1,:),waypts(2,:),'c','LineWidth',30);hold on; +plot(new_waypts(1,:),new_waypts(2,:),'.g');hold on; +plot(waypts(1,:),waypts(2,:),'*r');hold on; +% plot(waypts(1,:),waypts(2,:),'--b'); +for i=1:size(new_waypts,2) + plot_rect(new_waypts(:,i),r); +end + +title('minimum snap trajectory with corridor'); +tt = 0:0.01:T; +xx = polys_vals(polys_x,ts,tt,0); +yy = polys_vals(polys_y,ts,tt,0); +plot(xx,yy,'r'); + +end + +function plot_rect(center,r) +p1 = center+[-r;-r]; +p2 = center+[-r;r]; +p3 = center+[r;r]; +p4 = center+[r;-r]; +plot_line(p1,p2); +plot_line(p2,p3); +plot_line(p3,p4); +plot_line(p4,p1); +end + +function plot_line(p1,p2) +a = [p1(:),p2(:)]; +plot(a(1,:),a(2,:),'b'); +end + +function polys = minimum_snap_single_axis_corridor(waypts,ts,n_order,v0,a0,ve,ae,corridor_r) +p0 = waypts(1); +pe = waypts(end); + +n_poly = length(waypts)-1; +n_coef = n_order+1; + +% compute Q +Q_all = []; +for i=1:n_poly + Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1))); +end +b_all = zeros(size(Q_all,1),1); + +Aeq = zeros(3*n_poly+3,n_coef*n_poly); +beq = zeros(3*n_poly+3,1); + +% start/terminal pva constraints (6 equations) +Aeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0); + calc_tvec(ts(1),n_order,1); + calc_tvec(ts(1),n_order,2)]; +Aeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ... + [calc_tvec(ts(end),n_order,0); + calc_tvec(ts(end),n_order,1); + calc_tvec(ts(end),n_order,2)]; +beq(1:6,1) = [p0,v0,a0,pe,ve,ae]'; +neq = 6; + +% continuous constraints ((n_poly-1)*3 equations) +for i=1:n_poly-1 + tvec_p = calc_tvec(ts(i+1),n_order,0); + tvec_v = calc_tvec(ts(i+1),n_order,1); + tvec_a = calc_tvec(ts(i+1),n_order,2); + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p]; + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v]; + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a]; +end + +% corridor constraints (n_ploy-1 iequations) +Aieq = zeros(2*(n_poly-1),n_coef*n_poly); +bieq = zeros(2*(n_poly-1),1); +for i=1:n_poly-1 + tvec_p = calc_tvec(ts(i+1),n_order,0); + Aieq(2*i-1:2*i,n_coef*i+1:n_coef*(i+1)) = [tvec_p;-tvec_p]; + bieq(2*i-1:2*i) = [waypts(i+1)+corridor_r corridor_r-waypts(i+1)]; +end + +p = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq); + +polys = reshape(p,n_coef,n_poly); + +end + diff --git a/minimum_snap_trajectory_generation/demo3_minimum_snap_close_form.m b/minimum_snap_trajectory_generation/demo3_minimum_snap_close_form.m new file mode 100644 index 0000000..4e9949a --- /dev/null +++ b/minimum_snap_trajectory_generation/demo3_minimum_snap_close_form.m @@ -0,0 +1,140 @@ +function demo3_minimum_snap_close_form() +clear,clc; +addpath estimate transform +%% condition +pf = [-1.5 1.5 1; 1.8 2 1.5; 1.8 -4 1.5;1 6 0.5; 1.5 0 1.5; -1.5 1.5 0.5;]*3; + +v0 = [0,0,0]; +a0 = [0,0,0]; +j0 = [0,0,0]; +v1 = [0,0,0]; +a1 = [0,0,0]; +j1 = [0,0,0]; + +mV = [5 2]; +mA = [3 2]; +%% estimate and insert +%---Set up line to be fitted +data_size = size(pf,1); + +Ts = 0; +for i = 1:data_size-1 + TM = shortestTime_synced(pf(i,:),pf(i+1,:),mV,mA)*1.2; + Ts = [Ts; Ts(end)+TM]; +end +T = Ts(end); + +%---Set up the points to be fitted +insert_dist = 8; +Ds = []; +Dt = []; +for i=1:size(pf,1)-1 + seg_s=[]; + n_sample = 2 + round(norm(pf(i,:)-pf(i+1,:))/insert_dist); + for j=1:3 + seg_s(j,:) = linspace(pf(i,j),pf(i+1,j),n_sample); + end + seg_t = linspace(Ts(i),Ts(i+1),n_sample); + seg_s(:,end)=[]; + seg_t(end)=[]; + Ds = [Ds;seg_s']; + Dt = [Dt;seg_t']; +end +Ds = [Ds;pf(end,:)]; +Dt = [Dt;Ts(end)]; +%% +waypts = Ds'; +ts = Dt'; +n_order=7; +%% trajectory plan +polys_x = minimum_snap_single_axis_close_form(waypts(1,:),ts,n_order,v0(1),a0(1),j0(1),v1(1),a1(1),j1(1)); +polys_y = minimum_snap_single_axis_close_form(waypts(2,:),ts,n_order,v0(2),a0(2),j0(2),v1(2),a1(2),j1(2)); +polys_z = minimum_snap_single_axis_close_form(waypts(3,:),ts,n_order,v0(3),a0(2),j0(3),v1(3),a1(3),j1(3)); + +%% result show +figure(1) +% plot(waypts(1,:),waypts(2,:),'*r');hold on; +plot3(waypts(1,:),waypts(2,:),waypts(3,:),'b*-');hold on;axis equal; +title('minimum snap trajectory'); +color = ['grc']; +A = []; +% for i=1:size(polys_x,2) + tt = 0:0.01:Dt(end); + xx = polys_vals(polys_x,ts,tt,0); + yy = polys_vals(polys_y,ts,tt,0); + zz = polys_vals(polys_z,ts,tt,0); + A =[A [polys_vals(polys_x,ts,tt,2);polys_vals(polys_y,ts,tt,2);polys_vals(polys_z,ts,tt,2);]]; + plot3(xx,yy,zz,color(mod(i,3)+1)); +% end +figure(2);plot(A'); + +end + +function polys = minimum_snap_single_axis_close_form(wayp,ts,n_order,v0,a0,j0,v1,a1,j1) +n_coef = n_order+1; +n_poly = length(wayp)-1; +% compute Q +Q_all = []; +for i=1:n_poly + Q_all = blkdiag(Q_all,computeQ(n_order,4,ts(i),ts(i+1))); +end + +% compute Tk Tk(i,j) = ts(i)^(j-1) +tk = zeros(n_poly+1,n_coef); +for i = 1:n_coef + tk(:,i) = ts(:).^(i-1); +end + +% compute A (n_continuous*2*n_poly) * (n_coef*n_poly) +n_continuous = 4; % 1:p 2:pv 3:pva 4:pvaj 5:pvajs +A = zeros(n_continuous*2*n_poly,n_coef*n_poly); +for i = 1:n_poly + for j = 1:n_continuous + for k = j:n_coef + if k==j + t1 = 1; + t2 = 1; + else %k>j + t1 = tk(i,k-j+1); + t2 = tk(i+1,k-j+1); + end + A(n_continuous*2*(i-1)+j,n_coef*(i-1)+k) = prod(k-j+1:k-1)*t1; + A(n_continuous*2*(i-1)+n_continuous+j,n_coef*(i-1)+k) = prod(k-j+1:k-1)*t2; + end + end +end + +% compute M +M = zeros(n_poly*2*n_continuous,n_continuous*(n_poly+1)); +for i = 1:n_poly*2 + j = floor(i/2)+1; + rbeg = n_continuous*(i-1)+1; + cbeg = n_continuous*(j-1)+1; + M(rbeg:rbeg+n_continuous-1,cbeg:cbeg+n_continuous-1) = eye(n_continuous); +end + +% compute C +num_d = n_continuous*(n_poly+1); +C = eye(num_d); +df = [wayp,v0,a0,j0,v1,a1,j1]';% fix all pos(n_poly+1) + start va(2) + end va(2) +fix_idx = [1:4:num_d,2,3,4,num_d-2,num_d-1,num_d]; +free_idx = setdiff(1:num_d,fix_idx); +C = [C(:,fix_idx) C(:,free_idx)]; + +AiMC = inv(A)*M*C; +R = AiMC'*Q_all*AiMC; + +n_fix = length(fix_idx); +Rff = R(1:n_fix,1:n_fix); +Rpp = R(n_fix+1:end,n_fix+1:end); +Rfp = R(1:n_fix,n_fix+1:end); +Rpf = R(n_fix+1:end,1:n_fix); + +dp = -inv(Rpp)*Rfp'*df; + +p = AiMC*[df;dp]; + +polys = reshape(p,n_coef,n_poly); + +end + diff --git a/minimum_snap_trajectory_generation/demo4_minimum_snap_guiding.m b/minimum_snap_trajectory_generation/demo4_minimum_snap_guiding.m new file mode 100644 index 0000000..cc8a028 --- /dev/null +++ b/minimum_snap_trajectory_generation/demo4_minimum_snap_guiding.m @@ -0,0 +1,148 @@ +function demo4_minimum_snap_guiding() +clear,clc; + +%% condition +waypts = [0,0; + 1,2; + 2,0; + 4,5; + 5,2]'; +v0 = [0,0]; +a0 = [0,0]; +v1 = [0,0]; +a1 = [0,0]; +T = 5; +n_order = 5; +lambda = 10; %guiding weight + +%% sample mid points +r = 0.5; %% corridor r +step = r; +new_waypts = waypts(:,1); +for i=2:size(waypts,2) + x1 = waypts(1,i-1); + y1 = waypts(2,i-1); + x2 = waypts(1,i); + y2 = waypts(2,i); + n = ceil(hypot(x1-x2,y1-y2)/step)+1; + sample_pts = [linspace(x1,x2,n);linspace(y1,y2,n)]; + new_waypts = [new_waypts sample_pts(:,2:end)]; +end +ts = arrangeT(new_waypts,T); + +figure(1) +plot_idx = 221; +for lambda = [0,10,100,10000] + subplot(plot_idx); + %% trajectory plan + polys_x = minimum_snap_single_axis_guiding_path(new_waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1),r,lambda); + polys_y = minimum_snap_single_axis_guiding_path(new_waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2),r,lambda); + + %% result show + plot(new_waypts(1,:),new_waypts(2,:),'.g');hold on; + plot(waypts(1,:),waypts(2,:),'*r');hold on; + for i=1:size(new_waypts,2) + plot_rect(new_waypts(:,i),r); + end + title(['\lambda = ' num2str(lambda)]); + tt = 0:0.01:T; + xx = polys_vals(polys_x,ts,tt,0); + yy = polys_vals(polys_y,ts,tt,0); + plot(xx,yy,'r'); + plot_idx = plot_idx+1; +end + +end + +function plot_rect(center,r) +p1 = center+[-r;-r]; +p2 = center+[-r;r]; +p3 = center+[r;r]; +p4 = center+[r;-r]; +plot_line(p1,p2); +plot_line(p2,p3); +plot_line(p3,p4); +plot_line(p4,p1); +end + +function plot_line(p1,p2) +a = [p1(:),p2(:)]; +plot(a(1,:),a(2,:),'b'); +end + +function polys = minimum_snap_single_axis_guiding_path(waypts,ts,n_order,v0,a0,ve,ae,corridor_r,lambda) +p0 = waypts(1); +pe = waypts(end); + +n_poly = length(waypts)-1; +n_coef = n_order+1; + +% compute Q +Q_all = []; +for i=1:n_poly + Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1))); +end +b_all = zeros(size(Q_all,1),1); + +% add guiding pos +H_guide = []; +b_guide = []; +for i = 1:n_poly + t1 = ts(i); + t2 = ts(i+1); + p1 = waypts(i); + p2 = waypts(i+1); + a1 = (p2-p1)/(t2-t1); + a0 = p1-a1*t1; + ci = zeros(n_coef,1); + ci(1:2,1) = [a0;a1]; %guiding polynormial with linear curve + Qi = computeQ(n_order,0,t1,t2); + bi = -Qi'*ci; + H_guide = blkdiag(H_guide,Qi); + b_guide = [b_guide;bi]; +end +Q_all = Q_all+lambda*H_guide; +b_all = b_all+lambda*b_guide; + +Aeq = zeros(3*n_poly+3,n_coef*n_poly); +beq = zeros(3*n_poly+3,1); + +% start/terminal pva constraints (6 equations) +Aeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0); + calc_tvec(ts(1),n_order,1); + calc_tvec(ts(1),n_order,2)]; +Aeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ... + [calc_tvec(ts(end),n_order,0); + calc_tvec(ts(end),n_order,1); + calc_tvec(ts(end),n_order,2)]; +beq(1:6,1) = [p0,v0,a0,pe,ve,ae]'; +neq = 6; + +% continuous constraints ((n_poly-1)*3 equations) +for i=1:n_poly-1 + tvec_p = calc_tvec(ts(i+1),n_order,0); + tvec_v = calc_tvec(ts(i+1),n_order,1); + tvec_a = calc_tvec(ts(i+1),n_order,2); + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p]; + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v]; + neq=neq+1; + Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a]; +end + +% corridor constraints (n_ploy-1 iequations) +Aieq = zeros(2*(n_poly-1),n_coef*n_poly); +bieq = zeros(2*(n_poly-1),1); +for i=1:n_poly-1 + tvec_p = calc_tvec(ts(i+1),n_order,0); + Aieq(2*i-1:2*i,n_coef*i+1:n_coef*(i+1)) = [tvec_p;-tvec_p]; + bieq(2*i-1:2*i) = [waypts(i+1)+corridor_r corridor_r-waypts(i+1)]; +end + +p = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq); + +polys = reshape(p,n_coef,n_poly); + +end + diff --git a/minimum_snap_trajectory_generation/estimate/P2ndOrder.m b/minimum_snap_trajectory_generation/estimate/P2ndOrder.m new file mode 100644 index 0000000..5d752d0 --- /dev/null +++ b/minimum_snap_trajectory_generation/estimate/P2ndOrder.m @@ -0,0 +1,3 @@ +function [x] = P2ndOrder(t,x0,v0,a) + +x = x0+v0*t+0.5*a*t^2; \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/estimate/P3rdOrder.m b/minimum_snap_trajectory_generation/estimate/P3rdOrder.m new file mode 100644 index 0000000..7b0a7ec --- /dev/null +++ b/minimum_snap_trajectory_generation/estimate/P3rdOrder.m @@ -0,0 +1,3 @@ +function [x] = P3rdOrder(t,x0,v0,a0,j) + +x = x0+v0*t+0.5*a0*t^2+1/6*j*t^3; \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/estimate/ParaGen.m b/minimum_snap_trajectory_generation/estimate/ParaGen.m new file mode 100644 index 0000000..a147cc2 --- /dev/null +++ b/minimum_snap_trajectory_generation/estimate/ParaGen.m @@ -0,0 +1,79 @@ +function Params = ParaGen(vf, state, aM, jM) +%input: +%vf: velocity setpoint +%state: initial state +%aM,jM: max acc and jerk +% +%output: +%Params [T1,T2,T3,j1,j3,a(0~3),v(0~3),p(0~3)]: +%For reconstructing the trajectory + +% Calculate the velocity when acc driectly comes to zero +%------------------------------------------------------- +v_stop = state.a^2 / 2 / jM * sign(state.a) + state.v; + +% Deteremin the cruise direction and cruise acc +%------------------------------------------------------- +d = sign(vf - v_stop); +cruse_acc = d * aM; + +% Calculate the phase 1 params +%------------------------------------------------------- +t1 = abs(cruse_acc - state.a) / jM; +Params.j1 = jM * sign(cruse_acc-state.a); +v1 = state.v + state.a * t1 + 0.5 * Params.j1 * t1^2; + +% Calculate the pahse 3 params +%------------------------------------------------------- +t3 = abs(-cruse_acc) / jM; +Params.j3 = jM * sign(-cruse_acc); +v3bar = cruse_acc * t3 + 0.5 * Params.j3 * t3^2; + +% Calculate the phase 2 params +%------------------------------------------------------- +v2bar = vf - v1 - v3bar; + +if d == 0 + t2 = 0; +else + t2 = v2bar / cruse_acc; +end + +% There is a cruise phase iff t2 >= 0 +%------------------------------------------------------ +if t2 >= 0 + Params.T1 = t1; + Params.T2 = t1+t2; + Params.T3 = t1+t2+t3; +else + % Here a_norm stands for the maximum reachable acc if the cruse acc now + % cannot be reached + %---------------------------------------------------------------------- + a_norm = d * sqrt(d * jM * (vf - state.v) + 0.5 * state.a^2); + t1 = abs(a_norm - state.a) / jM; + t2 = 0; + t3 = abs(0 - a_norm)/jM; + Params.T1 = t1; + Params.T2 = t1+t2; + Params.T3 = t1+t2+t3; +end + +%Complete the params +%--------------------------------------------------------------------------- +Params.a0 = state.a; +Params.v0 = state.v; +Params.p0 = state.p; + +Params.a1 = V2ndOrder(Params.T1, Params.a0, Params.j1); +Params.v1 = P2ndOrder(Params.T1, Params.v0, Params.a0, Params.j1); +Params.p1 = P3rdOrder(Params.T1, Params.p0, Params.v0, Params.a0, Params.j1); + +Params.a2 = V2ndOrder(Params.T2 - Params.T1, Params.a1, 0); +Params.v2 = P2ndOrder(Params.T2 - Params.T1, Params.v1, Params.a1, 0); +Params.p2 = P3rdOrder(Params.T2 - Params.T1, Params.p1, Params.v1, Params.a1, 0); + +Params.a3 = V2ndOrder(Params.T3 - Params.T2, Params.a2, Params.j3); +Params.v3 = P2ndOrder(Params.T3 - Params.T2, Params.v2, Params.a2, Params.j3); +Params.p3 = P3rdOrder(Params.T3 - Params.T2, Params.p2, Params.v2, Params.a2, Params.j3); + +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/estimate/RefGen.m b/minimum_snap_trajectory_generation/estimate/RefGen.m new file mode 100644 index 0000000..0f466e2 --- /dev/null +++ b/minimum_snap_trajectory_generation/estimate/RefGen.m @@ -0,0 +1,20 @@ +%Generate the path reference using the given parameters +function [Ostate] = RefGen(Params, t) + +if t=Params.T1 && t=Params.T2 && tts(idx+1)+0.0001 + idx = idx+1; + end + vals(i) = poly_val(polys(:,idx),t,r); + end +end +end + diff --git a/minimum_snap_trajectory_generation/shortestTime_synced.m b/minimum_snap_trajectory_generation/shortestTime_synced.m new file mode 100644 index 0000000..4ecf869 --- /dev/null +++ b/minimum_snap_trajectory_generation/shortestTime_synced.m @@ -0,0 +1,12 @@ +function TM = shortestTime_synced(wp1,wp2,vM,aM) +%vM/aM = [horizon vertical]; +[G2L,L2G,yaw,pitch] = global2local(wp1',wp2'); +[vMc,aMc,jMc]=tiltConstraintsApprx(vM',aM',aM',pitch); + +state.p = 0; +state.v = 0; +state.a = 0; + +Params = ParaGen(norm(wp2-wp1), state, vMc(1), aMc(1)); +TM = Params.T3; +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/transform/FlightCorridor.m b/minimum_snap_trajectory_generation/transform/FlightCorridor.m new file mode 100644 index 0000000..b78dc4c --- /dev/null +++ b/minimum_snap_trajectory_generation/transform/FlightCorridor.m @@ -0,0 +1,130 @@ +classdef FlightCorridor < handle + %FLIGHTCORRIDOR: a light corridor defined in terms of a rotated cuboid. + + properties (Access = public) + sfc = [1,1,1]'; % The safe corridor distance to three dimensions + a = [0,0,0]'; % The start point + b = [0,0,0]'; % The end point + l = 0; % Corridor length + yaw = 0; + pitch = 0; + G2L; % Transformation matrix global to local + L2G; % Transformation matrix local to global + vMc; % Max velocity in local frame + aMc; % Max acceleration in local frame + jMc; % Max jerk in local frame + target; %Target in local frame + end + + methods + function this = FlightCorridor(a,b,sfc,vM,aM,jM) + this.a = a; + this.b = b; + this.sfc = sfc; + this.l = norm(b-a); + [this.G2L,this.L2G,this.yaw,this.pitch] = global2local(a,b); + [this.vMc,this.aMc,this.jMc]=tiltConstraintsApprx(vM,aM,jM,this.pitch); + this.target = [this.l 0 0]'; + end + %--- + function sfc = getSfc(this,i) + sfc = this.sfc(i); + end + %--- + function yaw = getYaw(this) + yaw = this.yaw; + end + %--- + function l = getL(this) + l = this.l; + end + %--- + function pitch = getPitch(this) + pitch = this.pitch; + end + %--- + function out = getVmc(this,i) + out = this.vMc(i); + end + %--- + function out = getAmc(this,i) + out = this.aMc(i); + end + %--- + function out = getJmc(this,i) + out = this.jMc(i); + end + %--- + function target = getTarget(this,i) + target = this.target(i); + end + %--- + function vec_out = l2g(this, vec_in,do_translation) + if(do_translation) + vec_out = transRot(this.L2G.pos, vec_in); + else + vec_out = transRot(this.L2G.free,vec_in); + end + end + %--- + function vec_out = g2l(this, vec_in,do_translation) + if(do_translation) + vec_out = transRot(this.G2L.pos, vec_in); + else + vec_out = transRot(this.G2L.free,vec_in); + end + end + %--- + function G2L = getG2L(this) + G2L = this.G2L; + end + %--- + function L2G = getL2G(this) + L2G = this.L2G; + end + %--- + function inside = isPointInside(this,p,is_local_cord) + if (~is_local_cord) + p = this.g2l(p); + end + + inside = this.isInsideLocalCuboid(p); + end + %--- + function inside = isInsideLocalCuboid(this,p) + inside = true; + %---Consider the all three face + if (p(1) < -this.sfc(1) || p(1) > this.sfc(1)+this.l) + inside = false; + return; + end + for i=2:3 + if(abs(p(i))>this.sfc(i)) + inside = false; + return; + end + end + end + %--- + function inside = isMaxMinValueInside(this,max_p,min_p) + inside = true; + + if max_p(1) < -this.sfc(1) || max_p(1) > this.sfc(1) + this.l + inside = false; + return; + end + if min_p(1) < -this.sfc(1) || min_p(1) > this.sfc(1) + this.l + inside = false; + return; + end + + for i=2:3 + if abs(max_p(i))>this.sfc(i) || abs(min_p(i))>this.sfc(i) + inside = false; + return; + end + end + end + end +end + diff --git a/minimum_snap_trajectory_generation/transform/global2local.m b/minimum_snap_trajectory_generation/transform/global2local.m new file mode 100644 index 0000000..46def62 --- /dev/null +++ b/minimum_snap_trajectory_generation/transform/global2local.m @@ -0,0 +1,43 @@ +function [G2L,L2G,yaw,pitch] = global2local(a,b) +%This function transfer the global pos vel acc into a corridor frame +%that its x-axis is along the line from a to b, its origin is at a, +%its y-axis is parrlel to the global y. +%---Input: +%a: the starting point of this corridor +%b: the end point of this corridor +%pos: position in global frame +%vel: velocity in global frame +%acc: acceleration in global frame +%---Output: +%G2L: the transformation matrix +%L2G: the reverse transformation matrix +%yaw: yaw angle in rad +%pitch: pitch angle in rad +%__________________________________________________________________________ + +vec = b - a; +%---Yaw rotation +yaw = atan2(vec(2),vec(1)); +%---forward +T_yaw = yawMatrix(yaw); +%---reverse +T_yaw_r = yawMatrix(-yaw); + +%---Pitch rotation +pitch = atan2(vec(3),norm(vec(1:2))); +%---forward +T_pitch = pitchMatrix(pitch); +%---reverse +T_pitch_r = pitchMatrix(-pitch); + +%---Translation +T_trans = transMatrix(-a); +T_trans_r = transMatrix(a); + +%---Outputs +G2L.free = T_pitch*T_yaw; +G2L.pos = T_pitch*T_yaw*T_trans; + +L2G.free = T_yaw_r*T_pitch_r; +L2G.pos = T_trans_r*T_yaw_r*T_pitch_r; + diff --git a/minimum_snap_trajectory_generation/transform/pitchMatrix.m b/minimum_snap_trajectory_generation/transform/pitchMatrix.m new file mode 100644 index 0000000..513ffa4 --- /dev/null +++ b/minimum_snap_trajectory_generation/transform/pitchMatrix.m @@ -0,0 +1,7 @@ +function A = pitchMatrix(pitch) +A = [cos(pitch), 0, sin(pitch), 0; + 0, 1, 0, 0; + -sin(pitch), 0, cos(pitch), 0; + 0, 0, 0, 1; + ]; +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/transform/switch2nextTarget.m b/minimum_snap_trajectory_generation/transform/switch2nextTarget.m new file mode 100644 index 0000000..7efb0e7 --- /dev/null +++ b/minimum_snap_trajectory_generation/transform/switch2nextTarget.m @@ -0,0 +1,28 @@ +function change2next = switch2nextTarget(SFCD,c,d,gs,aM,jM) +%whether vehicle is already in, whether the vehicle will stop in +isVehicle_in = 0; +isStop_in = 0; + +%project the current global states along the line c-d +[xs, ys, ~, ~] = ThreeDTransform(c', d', gs); + +%calculate whether the vehcile is within the c-d corridor +dist = norm(c-d); +if(xs.p>-SFCD && xs.p-SFCD && ys.p-SFCD && stp_x-SFCD && stp_y=h); +if(width_is_longer) + side_long = w; + side_short = h; +else + side_long = h; + side_short = w; +end + +% since the solutions for angle, -angle and 180-angle are all the same, +% if suffices to look at the first quadrant and the absolute values of sin,cos: +sin_a = abs(sin(angle)); +cos_a = abs(cos(angle)); + +if(side_short <= 2.0*sin_a*cos_a*side_long) +% half constrained case: two crop corners touch the longer side, +% the other two corners are on the mid-line parallel to the longer line + x = 0.5*side_short; + if(width_is_longer) + wr = x/sin_a; + hr = x/cos_a; + else + wr = x/cos_a; + hr = x/sin_a; + end +else +% fully constrained case: crop touches all 4 sides + cos_2a = cos_a*cos_a - sin_a*sin_a; + wr = (w*cos_a - h*sin_a)/cos_2a; + hr = (h*cos_a - w*sin_a)/cos_2a; +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/transform/transMatrix.m b/minimum_snap_trajectory_generation/transform/transMatrix.m new file mode 100644 index 0000000..cb51990 --- /dev/null +++ b/minimum_snap_trajectory_generation/transform/transMatrix.m @@ -0,0 +1,4 @@ +function A = transMatrix(v) +A = eye(4); +A(:,4) = [v;1]; +end \ No newline at end of file diff --git a/minimum_snap_trajectory_generation/transform/transRot.m b/minimum_snap_trajectory_generation/transform/transRot.m new file mode 100644 index 0000000..61d4d2d --- /dev/null +++ b/minimum_snap_trajectory_generation/transform/transRot.m @@ -0,0 +1,6 @@ +function c = transRot(A,b) +b_in = [b;1]; +c_in = A*b_in; +c = c_in(1:3); +end + diff --git a/minimum_snap_trajectory_generation/transform/yawMatrix.m b/minimum_snap_trajectory_generation/transform/yawMatrix.m new file mode 100644 index 0000000..c002c5a --- /dev/null +++ b/minimum_snap_trajectory_generation/transform/yawMatrix.m @@ -0,0 +1,7 @@ +function A = yawMatrix(yaw) +A = [cos(yaw), sin(yaw), 0, 0; + -sin(yaw), cos(yaw), 0, 0; + 0, 0, 1, 0; + 0, 0, 0, 1; + ]; +end \ No newline at end of file