[
  {
    "path": "README.md",
    "content": "# minimum snap trajectory planning in MATLAB\nThis repository contains sample code in MATLAB for minimum snap trajectory planning described in https://blog.csdn.net/q597967420/article/details/76099491.\nThis README provides a brief overview of our trajectory generation utilities with some examples.\n\nChinese Docs\n- Minimum Snap轨迹规划详解（1）轨迹规划入门.pdf\n- Minimum Snap轨迹规划详解（2）corridor与时间分配.pdf\n- Minimum Snap轨迹规划详解（3）闭式求解.pdf\n- Minimum Snap轨迹规划详解（4）guiding trajectory.pdf\n\n## Required\n1. MATLAB(R2013a is tested)\n\n## How to run\nThere are 4 demo codes in 2D space, you can directly run these and see results.\n\n - demo1: minimum snap trajectory planning with waypoints **strong constrants**(equality constraints).\n - demo2: minimum snap trajectory planning with **corridor constraints**(iequality constraints).\n - demo3: minimum snap trajectory planning with waypoints strong constrants by **close form** solution.\n - demo4: minimum snap trajectory planning with **guiding path**\n\n## Licence\nThe source code is released under GPLv3 license.\n\nAny problem, please contact maoshuyuan123@gmail.com\n"
  },
  {
    "path": "arrangeT.m",
    "content": "function ts = arrangeT(waypts,T)\n    x = waypts(:,2:end) - waypts(:,1:end-1);\n    dist = sum(x.^2,1).^0.5;\n    k = T/sum(dist);\n    ts = [0 cumsum(dist*k)];\nend"
  },
  {
    "path": "calc_tvec.m",
    "content": "% r=0:pos  1:vel  2:acc 3:jerk\nfunction tvec = calc_tvec(t,n_order,r)\n    tvec = zeros(1,n_order+1);\n    for i=r+1:n_order+1\n        tvec(i) = prod(i-r:i-1)*t^(i-r-1);\n    end\nend"
  },
  {
    "path": "computeQ.m",
    "content": "% n:polynormial order\n% r:derivertive order, 1:minimum vel 2:minimum acc 3:minimum jerk 4:minimum snap\n% t1:start timestamp for polynormial\n% t2:end timestap for polynormial\nfunction Q = computeQ(n,r,t1,t2)\nT = zeros((n-r)*2+1,1);\nfor i = 1:(n-r)*2+1\n    T(i) = t2^i-t1^i;\nend\nQ = zeros(n);\nfor i = r+1:n+1\n    for j = i:n+1\n        k1 = i-r-1;\n        k2 = j-r-1;\n        k = k1+k2+1;\n        Q(i,j) = prod(k1+1:k1+r)*prod(k2+1:k2+r)/k*T(k);\n        Q(j,i) = Q(i,j);\n    end\nend"
  },
  {
    "path": "demo1_minimum_snap_simple.m",
    "content": "function demo1_minimum_snap_simple()\n    clear,clc;\n\n    %% condition\n    waypts = [0,0;\n              1,2;\n              2,-1;\n              4,8;\n              5,2]';\n    v0 = [0,0];\n    a0 = [0,0];\n    v1 = [0,0];\n    a1 = [0,0];\n    T = 5;\n    ts = arrangeT(waypts,T);\n    n_order = 5;\n    \n    %% trajectory plan\n    polys_x = minimum_snap_single_axis_simple(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1));\n    polys_y = minimum_snap_single_axis_simple(waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2));\n    \n    %% result show\n    figure(1)\n    plot(waypts(1,:),waypts(2,:),'*r');hold on;\n    plot(waypts(1,:),waypts(2,:),'b--');\n    title('minimum snap trajectory');\n    color = ['grc'];\n    for i=1:size(polys_x,2)\n        tt = ts(i):0.01:ts(i+1);\n        xx = polys_vals(polys_x,ts,tt,0);\n        yy = polys_vals(polys_y,ts,tt,0);\n        plot(xx,yy,color(mod(i,3)+1));\n    end\n\n    figure(2)\n    vxx = polys_vals(polys_x,ts,tt,1);\n    axx = polys_vals(polys_x,ts,tt,2);\n    jxx = polys_vals(polys_x,ts,tt,3);\n    vyy = polys_vals(polys_y,ts,tt,1);\n    ayy = polys_vals(polys_y,ts,tt,2);\n    jyy = polys_vals(polys_y,ts,tt,3);\n    \n    subplot(421),plot(tt,xx);title('x position');\n    subplot(422),plot(tt,yy);title('y position');\n    subplot(423),plot(tt,vxx);title('x velocity');\n    subplot(424),plot(tt,vyy);title('y velocity');\n    subplot(425),plot(tt,axx);title('x acceleration');\n    subplot(426),plot(tt,ayy);title('y acceleration');\n    subplot(427),plot(tt,jxx);title('x jerk');\n    subplot(428),plot(tt,jyy);title('y jerk');\nend\n\n\nfunction polys = minimum_snap_single_axis_simple(waypts,ts,n_order,v0,a0,ve,ae)\np0 = waypts(1);\npe = waypts(end);\n\nn_poly = length(waypts)-1;\nn_coef = n_order+1;\n\n% compute Q\nQ_all = [];\nfor i=1:n_poly\n    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));\nend\nb_all = zeros(size(Q_all,1),1);\n\nAeq = zeros(4*n_poly+2,n_coef*n_poly);\nbeq = zeros(4*n_poly+2,1);\n\n% start/terminal pva constraints  (6 equations)\nAeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0);\n                     calc_tvec(ts(1),n_order,1);\n                     calc_tvec(ts(1),n_order,2)];\nAeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...\n                    [calc_tvec(ts(end),n_order,0);\n                     calc_tvec(ts(end),n_order,1);\n                     calc_tvec(ts(end),n_order,2)];\nbeq(1:6,1) = [p0,v0,a0,pe,ve,ae]';\n\n% mid p constraints    (n_ploy-1 equations)\nneq = 6;\nfor i=1:n_poly-1\n    neq=neq+1;\n    Aeq(neq,n_coef*i+1:n_coef*(i+1)) = calc_tvec(ts(i+1),n_order,0);\n    beq(neq) = waypts(i+1);\nend\n\n% continuous constraints  ((n_poly-1)*3 equations)\nfor i=1:n_poly-1\n    tvec_p = calc_tvec(ts(i+1),n_order,0);\n    tvec_v = calc_tvec(ts(i+1),n_order,1);\n    tvec_a = calc_tvec(ts(i+1),n_order,2);\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p];\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v];\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a];\nend\n\nAieq = [];\nbieq = [];\n\np = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq);\n\npolys = reshape(p,n_coef,n_poly);\n\nend\n\n"
  },
  {
    "path": "demo2_minimum_snap_corridor.m",
    "content": "function demo2_minimum_snap_corridor()\nclear,clc;\n\n%% condition\nwaypts = [0,0;\n          1,2;\n          2,0;\n          4,5;\n          5,2]';\nv0 = [0,0];\na0 = [0,0];\nv1 = [0,0];\na1 = [0,0];\nT = 5;\nn_order = 5;\n\n%% sample mid points\nr = 0.2;  %% corridor r\nstep = r;\nnew_waypts = waypts(:,1);\nfor i=2:size(waypts,2)\n    x1 = waypts(1,i-1);\n    y1 = waypts(2,i-1);\n    x2 = waypts(1,i);\n    y2 = waypts(2,i);\n    n = ceil(hypot(x1-x2,y1-y2)/step)+1;\n    sample_pts = [linspace(x1,x2,n);linspace(y1,y2,n)];\n    new_waypts = [new_waypts sample_pts(:,2:end)];\nend\nts = arrangeT(new_waypts,T);\n    \n%% trajectory plan\npolys_x = minimum_snap_single_axis_corridor(new_waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1),r);\npolys_y = minimum_snap_single_axis_corridor(new_waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2),r);\n\n%% result show\nfigure(1)\n% plot(waypts(1,:),waypts(2,:),'c','LineWidth',30);hold on;\nplot(new_waypts(1,:),new_waypts(2,:),'.g');hold on;\nplot(waypts(1,:),waypts(2,:),'*r');hold on;\n% plot(waypts(1,:),waypts(2,:),'--b');\nfor i=1:size(new_waypts,2)\n    plot_rect(new_waypts(:,i),r);\nend\n\ntitle('minimum snap trajectory with corridor');\ntt = 0:0.01:T;\nxx = polys_vals(polys_x,ts,tt,0);\nyy = polys_vals(polys_y,ts,tt,0);\nplot(xx,yy,'r');\n\nend\n\nfunction plot_rect(center,r)\np1 = center+[-r;-r];\np2 = center+[-r;r];\np3 = center+[r;r];\np4 = center+[r;-r];\nplot_line(p1,p2);\nplot_line(p2,p3);\nplot_line(p3,p4);\nplot_line(p4,p1);\nend\n\nfunction plot_line(p1,p2)\na = [p1(:),p2(:)];    \nplot(a(1,:),a(2,:),'b');\nend\n\nfunction polys = minimum_snap_single_axis_corridor(waypts,ts,n_order,v0,a0,ve,ae,corridor_r)\np0 = waypts(1);\npe = waypts(end);\n\nn_poly = length(waypts)-1;\nn_coef = n_order+1;\n\n% compute Q\nQ_all = [];\nfor i=1:n_poly\n    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));\nend\nb_all = zeros(size(Q_all,1),1);\n\nAeq = zeros(3*n_poly+3,n_coef*n_poly);\nbeq = zeros(3*n_poly+3,1);\n\n% start/terminal pva constraints  (6 equations)\nAeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0);\n                     calc_tvec(ts(1),n_order,1);\n                     calc_tvec(ts(1),n_order,2)];\nAeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...\n                    [calc_tvec(ts(end),n_order,0);\n                     calc_tvec(ts(end),n_order,1);\n                     calc_tvec(ts(end),n_order,2)];\nbeq(1:6,1) = [p0,v0,a0,pe,ve,ae]';\nneq = 6;\n\n% continuous constraints  ((n_poly-1)*3 equations)\nfor i=1:n_poly-1\n    tvec_p = calc_tvec(ts(i+1),n_order,0);\n    tvec_v = calc_tvec(ts(i+1),n_order,1);\n    tvec_a = calc_tvec(ts(i+1),n_order,2);\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p];\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v];\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a];\nend\n\n% corridor constraints (n_ploy-1 iequations)\nAieq = zeros(2*(n_poly-1),n_coef*n_poly);\nbieq = zeros(2*(n_poly-1),1);\nfor i=1:n_poly-1\n    tvec_p = calc_tvec(ts(i+1),n_order,0);\n    Aieq(2*i-1:2*i,n_coef*i+1:n_coef*(i+1)) = [tvec_p;-tvec_p];\n    bieq(2*i-1:2*i) = [waypts(i+1)+corridor_r corridor_r-waypts(i+1)];\nend\n\np = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq);\n\npolys = reshape(p,n_coef,n_poly);\n\nend\n\n"
  },
  {
    "path": "demo3_minimum_snap_close_form.m",
    "content": "function demo3_minimum_snap_close_form()\nclear,clc;\n\n%% condition\nwaypts = [0,0;\n          1,2;\n          2,-1;\n          4,8;\n          5,2]';\nv0 = [0,0];\na0 = [0,0];\nv1 = [0,0];\na1 = [0,0];\nT = 5;\nts = arrangeT(waypts,T);\nn_order = 5;\n\n%% trajectory plan\npolys_x = minimum_snap_single_axis_close_form(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1));\npolys_y = minimum_snap_single_axis_close_form(waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2));\n\n%% result show\nfigure(1)\nplot(waypts(1,:),waypts(2,:),'*r');hold on;\nplot(waypts(1,:),waypts(2,:),'b--');\ntitle('minimum snap trajectory');\ncolor = ['grc'];\nfor i=1:size(polys_x,2)\n    tt = ts(i):0.01:ts(i+1);\n    xx = polys_vals(polys_x,ts,tt,0);\n    yy = polys_vals(polys_y,ts,tt,0);\n    plot(xx,yy,color(mod(i,3)+1));\nend\n\nend\n\nfunction polys = minimum_snap_single_axis_close_form(wayp,ts,n_order,v0,a0,v1,a1)\nn_coef = n_order+1;\nn_poly = length(wayp)-1;\n% compute Q\nQ_all = [];\nfor i=1:n_poly\n    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));\nend\n\n% compute Tk   Tk(i,j) = ts(i)^(j-1)\ntk = zeros(n_poly+1,n_coef);\nfor i = 1:n_coef\n    tk(:,i) = ts(:).^(i-1);\nend\n\n% compute A (n_continuous*2*n_poly) * (n_coef*n_poly)\nn_continuous = 3;  % 1:p  2:pv  3:pva  4:pvaj  5:pvajs\nA = zeros(n_continuous*2*n_poly,n_coef*n_poly);\nfor i = 1:n_poly\n    for j = 1:n_continuous\n        for k = j:n_coef\n            if k==j\n                t1 = 1;\n                t2 = 1;\n            else %k>j\n                t1 = tk(i,k-j+1);\n                t2 = tk(i+1,k-j+1);\n            end\n            A(n_continuous*2*(i-1)+j,n_coef*(i-1)+k) = prod(k-j+1:k-1)*t1;\n            A(n_continuous*2*(i-1)+n_continuous+j,n_coef*(i-1)+k) = prod(k-j+1:k-1)*t2;\n        end\n    end\nend\n\n% compute M\nM = zeros(n_poly*2*n_continuous,n_continuous*(n_poly+1));\nfor i = 1:n_poly*2\n    j = floor(i/2)+1;\n    rbeg = n_continuous*(i-1)+1;\n    cbeg = n_continuous*(j-1)+1;\n    M(rbeg:rbeg+n_continuous-1,cbeg:cbeg+n_continuous-1) = eye(n_continuous);\nend\n\n% compute C\nnum_d = n_continuous*(n_poly+1);\nC = eye(num_d);\ndf = [wayp,v0,a0,v1,a1]';% fix all pos(n_poly+1) + start va(2) + end va(2) \nfix_idx = [1:3:num_d,2,3,num_d-1,num_d];\nfree_idx = setdiff(1:num_d,fix_idx);\nC = [C(:,fix_idx) C(:,free_idx)];\n\nAiMC = inv(A)*M*C;\nR = AiMC'*Q_all*AiMC;\n\nn_fix = length(fix_idx);\nRff = R(1:n_fix,1:n_fix);\nRpp = R(n_fix+1:end,n_fix+1:end);\nRfp = R(1:n_fix,n_fix+1:end);\nRpf = R(n_fix+1:end,1:n_fix);\n\ndp = -inv(Rpp)*Rfp'*df;\n\np = AiMC*[df;dp];\n\npolys = reshape(p,n_coef,n_poly);\n\nend\n\n"
  },
  {
    "path": "demo4_minimum_snap_guiding.m",
    "content": "function demo4_minimum_snap_guiding()\nclear,clc;\n\n%% condition\nwaypts = [0,0;\n          1,2;\n          2,0;\n          4,5;\n          5,2]';\nv0 = [0,0];\na0 = [0,0];\nv1 = [0,0];\na1 = [0,0];\nT = 5;\nn_order = 5;\nlambda = 10; %guiding weight\n\n%% sample mid points\nr = 0.5;  %% corridor r\nstep = r;\nnew_waypts = waypts(:,1);\nfor i=2:size(waypts,2)\n    x1 = waypts(1,i-1);\n    y1 = waypts(2,i-1);\n    x2 = waypts(1,i);\n    y2 = waypts(2,i);\n    n = ceil(hypot(x1-x2,y1-y2)/step)+1;\n    sample_pts = [linspace(x1,x2,n);linspace(y1,y2,n)];\n    new_waypts = [new_waypts sample_pts(:,2:end)];\nend\nts = arrangeT(new_waypts,T);\n\nfigure(1)\nplot_idx = 221;\nfor lambda = [0,10,100,10000]\n    subplot(plot_idx);\n    %% trajectory plan\n    polys_x = minimum_snap_single_axis_guiding_path(new_waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1),r,lambda);\n    polys_y = minimum_snap_single_axis_guiding_path(new_waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2),r,lambda);\n\n    %% result show\n    plot(new_waypts(1,:),new_waypts(2,:),'.g');hold on;\n    plot(waypts(1,:),waypts(2,:),'*r');hold on;\n    for i=1:size(new_waypts,2)\n        plot_rect(new_waypts(:,i),r);\n    end\n    title(['\\lambda = ' num2str(lambda)]);\n    tt = 0:0.01:T;\n    xx = polys_vals(polys_x,ts,tt,0);\n    yy = polys_vals(polys_y,ts,tt,0);\n    plot(xx,yy,'r');\n    plot_idx = plot_idx+1;\nend\n\nend\n\nfunction plot_rect(center,r)\np1 = center+[-r;-r];\np2 = center+[-r;r];\np3 = center+[r;r];\np4 = center+[r;-r];\nplot_line(p1,p2);\nplot_line(p2,p3);\nplot_line(p3,p4);\nplot_line(p4,p1);\nend\n\nfunction plot_line(p1,p2)\na = [p1(:),p2(:)];    \nplot(a(1,:),a(2,:),'b');\nend\n\nfunction polys = minimum_snap_single_axis_guiding_path(waypts,ts,n_order,v0,a0,ve,ae,corridor_r,lambda)\np0 = waypts(1);\npe = waypts(end);\n\nn_poly = length(waypts)-1;\nn_coef = n_order+1;\n\n% compute Q\nQ_all = [];\nfor i=1:n_poly\n    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));\nend\nb_all = zeros(size(Q_all,1),1);\n\n% add guiding pos\nH_guide = [];\nb_guide = [];\nfor i = 1:n_poly\n    t1 = ts(i);\n    t2 = ts(i+1);\n    p1 = waypts(i);\n    p2 = waypts(i+1);\n    a1 = (p2-p1)/(t2-t1);\n    a0 = p1-a1*t1;\n    ci = zeros(n_coef,1);\n    ci(1:2,1) = [a0;a1];  %guiding polynormial with linear curve\n    Qi = computeQ(n_order,0,t1,t2);\n    bi = -Qi'*ci;\n    H_guide = blkdiag(H_guide,Qi);\n    b_guide = [b_guide;bi];\nend\nQ_all = Q_all+lambda*H_guide;\nb_all = b_all+lambda*b_guide;\n\nAeq = zeros(3*n_poly+3,n_coef*n_poly);\nbeq = zeros(3*n_poly+3,1);\n\n% start/terminal pva constraints  (6 equations)\nAeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0);\n                     calc_tvec(ts(1),n_order,1);\n                     calc_tvec(ts(1),n_order,2)];\nAeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...\n                    [calc_tvec(ts(end),n_order,0);\n                     calc_tvec(ts(end),n_order,1);\n                     calc_tvec(ts(end),n_order,2)];\nbeq(1:6,1) = [p0,v0,a0,pe,ve,ae]';\nneq = 6;\n\n% continuous constraints  ((n_poly-1)*3 equations)\nfor i=1:n_poly-1\n    tvec_p = calc_tvec(ts(i+1),n_order,0);\n    tvec_v = calc_tvec(ts(i+1),n_order,1);\n    tvec_a = calc_tvec(ts(i+1),n_order,2);\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p];\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v];\n    neq=neq+1;\n    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a];\nend\n\n% corridor constraints (n_ploy-1 iequations)\nAieq = zeros(2*(n_poly-1),n_coef*n_poly);\nbieq = zeros(2*(n_poly-1),1);\nfor i=1:n_poly-1\n    tvec_p = calc_tvec(ts(i+1),n_order,0);\n    Aieq(2*i-1:2*i,n_coef*i+1:n_coef*(i+1)) = [tvec_p;-tvec_p];\n    bieq(2*i-1:2*i) = [waypts(i+1)+corridor_r corridor_r-waypts(i+1)];\nend\n\np = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq);\n\npolys = reshape(p,n_coef,n_poly);\n\nend\n\n"
  },
  {
    "path": "poly_val.m",
    "content": "function val = poly_val(poly,t,r)\n    val = 0;\n    n = length(poly)-1;\n    if r<=0\n        for i=0:n\n            val = val+poly(i+1)*t^i;\n        end\n    else\n        for i=r:n\n            a = poly(i+1)*prod(i-r+1:i)*t^(i-r);\n            val = val + a;\n        end\n    end\nend"
  },
  {
    "path": "polys_vals.m",
    "content": "function vals = polys_vals(polys,ts,tt,r)\nidx = 1;\nN = length(tt);\nvals = zeros(1,N);\nfor i = 1:N\n    t = tt(i);\n    if t<ts(idx)\n        vals(i) = 0;\n    else\n        while idx<length(ts) && t>ts(idx+1)+0.0001\n            idx = idx+1;\n        end\n        vals(i) = poly_val(polys(:,idx),t,r);\n    end\nend\nend\n\n"
  }
]