Full Code of symao/minimum_snap_trajectory_generation for AI

Repository: symao/minimum_snap_trajectory_generation
Branch: master
Commit: 73137c776479
Files: 10
Total size: 14.6 KB

Directory structure:
gitextract_0b88qo0l/

├── README.md
├── arrangeT.m
├── calc_tvec.m
├── computeQ.m
├── demo1_minimum_snap_simple.m
├── demo2_minimum_snap_corridor.m
├── demo3_minimum_snap_close_form.m
├── demo4_minimum_snap_guiding.m
├── poly_val.m
└── polys_vals.m

================================================
FILE CONTENTS
================================================

================================================
FILE: README.md
================================================
# minimum snap trajectory planning in MATLAB
This repository contains sample code in MATLAB for minimum snap trajectory planning described in https://blog.csdn.net/q597967420/article/details/76099491.
This README provides a brief overview of our trajectory generation utilities with some examples.

Chinese Docs
- Minimum Snap轨迹规划详解（1）轨迹规划入门.pdf
- Minimum Snap轨迹规划详解（2）corridor与时间分配.pdf
- Minimum Snap轨迹规划详解（3）闭式求解.pdf
- Minimum Snap轨迹规划详解（4）guiding trajectory.pdf

## 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


================================================
FILE: arrangeT.m
================================================
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

================================================
FILE: calc_tvec.m
================================================
% 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

================================================
FILE: computeQ.m
================================================
% 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

================================================
FILE: demo1_minimum_snap_simple.m
================================================
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



================================================
FILE: demo2_minimum_snap_corridor.m
================================================
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



================================================
FILE: demo3_minimum_snap_close_form.m
================================================
function demo3_minimum_snap_close_form()
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_close_form(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1));
polys_y = minimum_snap_single_axis_close_form(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

end

function polys = minimum_snap_single_axis_close_form(wayp,ts,n_order,v0,a0,v1,a1)
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,3,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 = 3;  % 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,v1,a1]';% fix all pos(n_poly+1) + start va(2) + end va(2) 
fix_idx = [1:3:num_d,2,3,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



================================================
FILE: demo4_minimum_snap_guiding.m
================================================
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



================================================
FILE: poly_val.m
================================================
function val = poly_val(poly,t,r)
    val = 0;
    n = length(poly)-1;
    if r<=0
        for i=0:n
            val = val+poly(i+1)*t^i;
        end
    else
        for i=r:n
            a = poly(i+1)*prod(i-r+1:i)*t^(i-r);
            val = val + a;
        end
    end
end

================================================
FILE: polys_vals.m
================================================
function vals = polys_vals(polys,ts,tt,r)
idx = 1;
N = length(tt);
vals = zeros(1,N);
for i = 1:N
    t = tt(i);
    if t<ts(idx)
        vals(i) = 0;
    else
        while idx<length(ts) && t>ts(idx+1)+0.0001
            idx = idx+1;
        end
        vals(i) = poly_val(polys(:,idx),t,r);
    end
end
end