├── QP_trajec_generation.m ├── README.md ├── computeA.m ├── computeConstraint.m ├── constraintData.m ├── drawSolution.m ├── keyframe.mat ├── keyframe_selection.m ├── main.m ├── mouseClick1.m ├── mouseClick2.m ├── mouseMove1.m ├── mouseMove2.m └── parameters.m /QP_trajec_generation.m: -------------------------------------------------------------------------------- 1 | c = zeros(4*(order+1)*m); %coefficients of polynomial functions 2 | 3 | 4 | %Quadratic cost function (minimum snap) : J = c.' * A * c 5 | mu_r = 1; mu_psi = 1; %constants that makes the integrand nondimensional 6 | k_r = 4; k_psi = 2; 7 | 8 | %Compute the cost function matrix, A 9 | A = computeA(order, m, mu_r, mu_psi, k_r, k_psi, t); 10 | 11 | %Compute the constraint function matrix, C 12 | % [C, b] = computeConstraint(order, m, k_r, k_psi, t, keyframe); 13 | [C, b, Cin, bin] = computeConstraint(order, m, 3, 2, t, keyframe, corridor_position, n_intermediate, corridor_width); 14 | 15 | options = optimoptions('quadprog', 'Display', 'iter', 'MaxIterations', 4000); 16 | tic; 17 | solution_corridor = quadprog(2*A, [], Cin, bin, C, b, [], [], [], options); 18 | toc; 19 | tic; 20 | solution = quadprog(2*A, [], [], [], C, b, [], [], [], options); 21 | toc; 22 | % solution = quadprog(2*A, [], [], [], [], [], [], [], [], options); -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # minSnapTrajec_matlab 2 | MATLAB simulation for the paper 'Minimum snap trajectory generation and control for quadrotors' 3 | 4 | ICSL drone flight project team 5 | 6 | ### Members ### 7 | + Youngdong Clark Son 8 | * email : clark.y.d.son@gmail.com 9 | 10 | ### Manual ### 11 | 1. Run main.m file in the MATLAB. 12 | 2. Click m(in parameters.m file) waypoints int the left top axis, which will assign x and y values of the waypoints. 13 | 3. Click m altitudes of the waypoints you chose in the previous step. 14 | 4. QP program will run automatically according to the parameters in the 'parameters.m' file. 15 | 5. Plots of the result show two result where the black one is the result without corridor constraints while red one is with them. 16 | -------------------------------------------------------------------------------- /computeA.m: -------------------------------------------------------------------------------- 1 | function A = computeA(order, m, mu_r, mu_psi, k_r, k_psi, t) 2 | 3 | polynomial_r = ones(1,order+1); 4 | for i=1:k_r 5 | polynomial_r = polyder(polynomial_r); %Differentiation 6 | end 7 | 8 | polynomial_psi = ones(1,order+1); 9 | for i=1:k_psi 10 | polynomial_psi = polyder(polynomial_psi); %Differentiation 11 | end 12 | 13 | A = []; 14 | for i=1:m 15 | A_x = zeros(order+1,order+1); 16 | A_y = zeros(order+1,order+1); 17 | A_z = zeros(order+1,order+1); 18 | A_psi = zeros(order+1,order+1); 19 | for j=1:order+1 20 | for k=j:order+1 21 | 22 | %Position 23 | if(j<=length(polynomial_r) && (k<=length(polynomial_r))) 24 | order_t_r = ((order-k_r-j+1)+(order-k_r-k+1)); 25 | if(j==k) 26 | A_x(j,k) = polynomial_r(j)^2/(order_t_r+1)... 27 | *(t(i+1)^(order_t_r+1)-t(i)^(order_t_r+1)); 28 | A_y(j,k) = polynomial_r(j)^2/(order_t_r+1)... 29 | *(t(i+1)^(order_t_r+1)-t(i)^(order_t_r+1)); 30 | A_z(j,k) = polynomial_r(j)^2/(order_t_r+1)... 31 | *(t(i+1)^(order_t_r+1)-t(i)^(order_t_r+1)); 32 | else 33 | A_x(j,k) = 2*polynomial_r(j)*polynomial_r(k)/(order_t_r+1)... 34 | *(t(i+1)^(order_t_r+1)-t(i)^(order_t_r+1)); 35 | A_y(j,k) = 2*polynomial_r(j)*polynomial_r(k)/(order_t_r+1)... 36 | *(t(i+1)^(order_t_r+1)-t(i)^(order_t_r+1)); 37 | A_z(j,k) = 2*polynomial_r(j)*polynomial_r(k)/(order_t_r+1)... 38 | *(t(i+1)^(order_t_r+1)-t(i)^(order_t_r+1)); 39 | end 40 | end 41 | 42 | %Yaw 43 | if(j<=length(polynomial_psi) && (k<=length(polynomial_psi))) 44 | order_t_psi = ((order-k_psi-j+1)+(order-k_psi-k+1)); 45 | if(j==k) 46 | A_psi(j,k) = polynomial_psi(j)^2/(order_t_psi+1)... 47 | *(t(i+1)^(order_t_psi+1)-t(i)^(order_t_psi+1)); 48 | else 49 | A_psi(j,k) = 2*polynomial_psi(j)*polynomial_psi(k)/(order_t_psi+1)... 50 | *(t(i+1)^(order_t_psi+1)-t(i)^(order_t_psi+1)); 51 | end 52 | end 53 | 54 | end 55 | end 56 | A = blkdiag(A,mu_r*A_x,mu_r*A_y,mu_r*A_z,mu_psi*A_psi); 57 | end 58 | A = 0.5*(A + A.'); %Make it symmetric 59 | end -------------------------------------------------------------------------------- /computeConstraint.m: -------------------------------------------------------------------------------- 1 | function [C, b, C3, b3] = computeConstraint(order, m, k_r, k_psi, t, keyframe, corridor_position, n_intermediate, corridor_width) 2 | 3 | n = 4; %State number 4 | 5 | %Waypoint constraints 6 | C1 = zeros(2*m*n,n*(order+1)*m); 7 | b1 = zeros(2*m*n,1); 8 | computeMat = eye(order+1); %Required for computation of polynomials 9 | for i=1:m 10 | waypoint = keyframe(:,i); %Waypoint at t(i) 11 | 12 | if(i==1) %Initial and Final Position 13 | %Initial 14 | values = zeros(1,order+1); 15 | for j=1:order+1 16 | values(j) = polyval(computeMat(j,:),t(i)); 17 | end 18 | 19 | for k=1:n 20 | c = zeros(1,n*(order+1)*m); 21 | c( ((i-1)*(order+1)*n+(k-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 22 | C1(k,:) = c; 23 | end 24 | b1(1:n) = waypoint; 25 | 26 | %Final 27 | for j=1:order+1 28 | values(j) = polyval(computeMat(j,:),t(m+1)); 29 | end 30 | 31 | for k=1:n 32 | c = zeros(1,n*(order+1)*m); 33 | c( ((m-1)*(order+1)*n+(k-1)*(order+1)+1) : ((m-1)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 34 | C1(k+n,:) = c; 35 | end 36 | b1(n+1:2*n) = waypoint; 37 | 38 | else 39 | %Elsewhere 40 | values = zeros(1,order+1); 41 | for j=1:order+1 42 | values(j) = polyval(computeMat(j,:),t(i)); 43 | end 44 | 45 | for k=1:n 46 | c = zeros(1,n*(order+1)*m); 47 | c( ((i-2)*(order+1)*n+(k-1)*(order+1)+1) : ((i-2)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 48 | C1(k+2*n*(i-1),:) = c; 49 | end 50 | b1(2*n*(i-1)+1:2*n*(i-1)+n) = waypoint; 51 | 52 | for k=1:n 53 | c = zeros(1,n*(order+1)*m); 54 | c( ((i-1)*(order+1)*n+(k-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 55 | C1(k+2*n*(i-1)+n,:) = c; 56 | end 57 | b1(2*n*(i-1)+n+1:2*n*(i-1)+2*n) = waypoint; 58 | 59 | end 60 | 61 | end 62 | 63 | 64 | % Derivative constraints 65 | 66 | % Position 67 | C2 = zeros(2*m*(n-1)*k_r,n*(order+1)*m); %(n-1) : yaw excluded here 68 | b2 = ones(2*m*(n-1)*k_r,1)*eps; 69 | constraintData; 70 | %constraintData_r = zeros(m,k_r,3); 71 | for i=1:m 72 | for h=1:k_r 73 | if(i==1) 74 | %Initial 75 | values = zeros(1,order+1); 76 | for j=1:order+1 77 | tempCoeffs = computeMat(j,:); 78 | for k=1:h 79 | tempCoeffs = polyder(tempCoeffs); 80 | end 81 | values(j) = polyval(tempCoeffs,t(i)); 82 | end 83 | 84 | continuity = zeros(1,n-1); 85 | for k=1:n-1 86 | if(constraintData_r(i,h,k)==eps) 87 | %Continuity 88 | continuity(k) = true; 89 | end 90 | 91 | c = zeros(1,n*(order+1)*m); 92 | if(continuity(k)) 93 | c( ((i-1)*(order+1)*n+(k-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 94 | c( ((m-1)*(order+1)*n+(k-1)*(order+1)+1) : ((m-1)*(order+1)*n+(k-1)*(order+1))+order+1) = -values; 95 | C2(k + (h-1)*(n-1),:) = c; 96 | b2(k + (h-1)*(n-1)) = 0; 97 | else 98 | c( ((i-1)*(order+1)*n+(k-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 99 | C2(k + (h-1)*(n-1),:) = c; 100 | b2(k + (h-1)*(n-1)) = constraintData_r(i,h,k); 101 | end 102 | end 103 | 104 | %Final 105 | values = zeros(1,order+1); 106 | for j=1:order+1 107 | tempCoeffs = computeMat(j,:); 108 | for k=1:h 109 | tempCoeffs = polyder(tempCoeffs); 110 | end 111 | values(j) = polyval(tempCoeffs,t(m+1)); 112 | end 113 | 114 | for k=1:n-1 115 | if(constraintData_r(i,h,k)==eps) 116 | %Continuity 117 | end 118 | c = zeros(1,n*(order+1)*m); 119 | if(~continuity(k)) 120 | c( ((m-1)*(order+1)*n+(k-1)*(order+1)+1) : ((m-1)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 121 | C2(k + (h-1)*(n-1) + (n-1)*k_r,:) = c; 122 | b2(k + (h-1)*(n-1) + (n-1)*k_r) = constraintData_r(i,h,k); 123 | end 124 | end 125 | 126 | else 127 | 128 | %Elsewhere 129 | values = zeros(1,order+1); 130 | for j=1:order+1 131 | tempCoeffs = computeMat(j,:); 132 | for k=1:h 133 | tempCoeffs = polyder(tempCoeffs); 134 | end 135 | values(j) = polyval(tempCoeffs,t(i)); 136 | end 137 | 138 | continuity = zeros(1,n-1); 139 | for k=1:n-1 140 | if(constraintData_r(i,h,k)==eps) 141 | %Continuity 142 | continuity(k) = true; 143 | end 144 | 145 | c = zeros(1,n*(order+1)*m); 146 | if(continuity(k)) 147 | c( ((i-2)*(order+1)*n+(k-1)*(order+1)+1) : ((i-2)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 148 | c( ((i-1)*(order+1)*n+(k-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k-1)*(order+1))+order+1) = -values; 149 | C2(k + (h-1)*(n-1) + 2*(i-1)*(n-1)*k_r,:) = c; 150 | b2(k + (h-1)*(n-1) + 2*(i-1)*(n-1)*k_r) = 0; 151 | else 152 | c( ((i-2)*(order+1)*n+(k-1)*(order+1)+1) : ((i-2)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 153 | C2(k + (h-1)*(n-1) + 2*(i-1)*(n-1)*k_r,:) = c; 154 | b2(k + (h-1)*(n-1) + 2*(i-1)*(n-1)*k_r) = constraintData_r(i,h,k); 155 | end 156 | end 157 | 158 | continuity = zeros(1,n-1); 159 | for k=1:n-1 160 | if(constraintData_r(i,h,k)==eps) 161 | %Continuity 162 | continuity(k) = true; 163 | end 164 | c = zeros(1,n*(order+1)*m); 165 | 166 | if(~continuity(k)) 167 | c( ((i-1)*(order+1)*n+(k-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k-1)*(order+1))+order+1) = values; 168 | C2(k + (h-1)*(n-1) + 2*(i-1)*(n-1)*k_r + (n-1)*k_r,:) = c; 169 | b2(k + (h-1)*(n-1) + 2*(i-1)*(n-1)*k_r + (n-1)*k_r) = constraintData_r(i,h,k); 170 | end 171 | 172 | end 173 | 174 | end 175 | end 176 | end 177 | 178 | 179 | % % % % % % % Yaw 180 | % % % % % % C3 = zeros(2*m*k_psi,n*(order+1)*m); %(n-1) : yaw excluded here 181 | % % % % % % b3 = zeros(2*m*k_psi,1); 182 | % % % % % % constraintData; 183 | % % % % % % 184 | % % % % % % for i=1:m 185 | % % % % % % for h=1:k_psi 186 | % % % % % % if(i==1) 187 | % % % % % % %Initial 188 | % % % % % % values = zeros(1,order+1); 189 | % % % % % % for j=1:order+1 190 | % % % % % % tempCoeffs = computeMat(j,:); 191 | % % % % % % for k=1:h 192 | % % % % % % tempCoeffs = polyder(tempCoeffs); 193 | % % % % % % end 194 | % % % % % % values(j) = polyval(tempCoeffs,t(i)); 195 | % % % % % % end 196 | % % % % % % 197 | % % % % % % for k=1:1 198 | % % % % % % c = zeros(1,n*(order+1)*m); 199 | % % % % % % c( ((i-1)*(order+1)*n+(k+3-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k+3-1)*(order+1))+order+1) = values; 200 | % % % % % % C3(k + (h-1)*(1),:) = c; 201 | % % % % % % b3(k + (h-1)*(1)) = constraintData_r(i,h,k); 202 | % % % % % % end 203 | % % % % % % 204 | % % % % % % %Final 205 | % % % % % % values = zeros(1,order+1); 206 | % % % % % % for j=1:order+1 207 | % % % % % % tempCoeffs = computeMat(j,:); 208 | % % % % % % for k=1:h 209 | % % % % % % tempCoeffs = polyder(tempCoeffs); 210 | % % % % % % end 211 | % % % % % % values(j) = polyval(tempCoeffs,t(m+1)); 212 | % % % % % % end 213 | % % % % % % 214 | % % % % % % for k=1:1 215 | % % % % % % c = zeros(1,n*(order+1)*m); 216 | % % % % % % c( ((m-1)*(order+1)*n+(k+3-1)*(order+1)+1) : ((m-1)*(order+1)*n+(k+3-1)*(order+1))+order+1) = values; 217 | % % % % % % C3(k + (h-1)*(1) + (1)*k_r,:) = c; 218 | % % % % % % b3(k + (h-1)*(1) + (1)*k_r) = constraintData_r(m,h,k); 219 | % % % % % % end 220 | % % % % % % 221 | % % % % % % else 222 | % % % % % % 223 | % % % % % % %Elsewhere 224 | % % % % % % % values = zeros(1,order+1); 225 | % % % % % % % for j=1:order+1 226 | % % % % % % % tempCoeffs = computeMat(j,:); 227 | % % % % % % % for k=1:h 228 | % % % % % % % tempCoeffs = polyder(tempCoeffs); 229 | % % % % % % % end 230 | % % % % % % % values(j) = polyval(tempCoeffs,t(i)); 231 | % % % % % % % end 232 | % % % % % % % 233 | % % % % % % % for k=1:1 234 | % % % % % % % c = zeros(1,n*(order+1)*m); 235 | % % % % % % % c( ((i-2)*(order+1)*n+(k+3-1)*(order+1)+1) : ((i-2)*(order+1)*n+(k+3-1)*(order+1))+order+1) = values; 236 | % % % % % % % C3(k + (h-1)*(1) + 2*(i-1)*(1)*k_r,:) = c; 237 | % % % % % % % b3(k + (h-1)*(1) + 2*(i-1)*(1)*k_r) = constraintData_r(i,h,k); 238 | % % % % % % % end 239 | % % % % % % % 240 | % % % % % % % for k=1:1 241 | % % % % % % % c = zeros(1,n*(order+1)*m); 242 | % % % % % % % c( ((i-1)*(order+1)*n+(k+3-1)*(order+1)+1) : ((i-1)*(order+1)*n+(k+3-1)*(order+1))+order+1) = values; 243 | % % % % % % % C3(k + (h-1)*(1) + 2*(i-1)*(1)*k_r + (1)*k_r,:) = c; 244 | % % % % % % % b3(k + (h-1)*(1) + 2*(i-1)*(1)*k_r + (1)*k_r) = constraintData_r(i,h,k); 245 | % % % % % % % end 246 | % % % % % % 247 | % % % % % % end 248 | % % % % % % end 249 | % % % % % % end 250 | 251 | %Corridor constraints 252 | 253 | C3 = []; 254 | %Size (2*3*n_intermediate,n*(order+1)*m); 255 | %2 : absoulute value constraint 256 | %3 : x,y,z 257 | %n_intermediate : number of intermediate points 258 | 259 | b3 = []; 260 | t_vector = (keyframe(1:3,corridor_position(2)) - keyframe(1:3,corridor_position(1)))... 261 | /norm(keyframe(1:3,corridor_position(2)) - keyframe(1:3,corridor_position(1))); 262 | %unit vector of direction of the corridor 263 | 264 | t_intermediate = linspace(t(corridor_position(1)),t(corridor_position(2)),n_intermediate+2); 265 | t_intermediate = t_intermediate(2:end-1); 266 | %intermediate time stamps 267 | 268 | computeMat = eye(order+1); %Required for computation of polynomials 269 | for i = 1:n_intermediate 270 | % intermediate_pos(1) = interp1([t(corridor_position(1)) t(corridor_position(2))],... 271 | % [keyframe(1,corridor_position(1)) keyframe(1,corridor_position(2))],t_intermediate(i)); 272 | % intermediate_pos(2) = interp1([t(corridor_position(1)) t(corridor_position(2))],... 273 | % [keyframe(2,corridor_position(1)) keyframe(2,corridor_position(2))],t_intermediate(i)); 274 | % intermediate_pos(3) = interp1([t(corridor_position(1)) t(corridor_position(2))],... 275 | % [keyframe(3,corridor_position(1)) keyframe(3,corridor_position(2))],t_intermediate(i)); 276 | values = zeros(1,order+1); 277 | for j=1:order+1 278 | values(j) = polyval(computeMat(j,:),t_intermediate(i)); 279 | end 280 | 281 | c = zeros(6, n*(order+1)*m); %Absolute value constraint : two inequality constraints 282 | b = zeros(6, 1); 283 | 284 | rix = keyframe(1,corridor_position(1)); 285 | riy = keyframe(2,corridor_position(1)); 286 | riz = keyframe(3,corridor_position(1)); 287 | %x 288 | c(1,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1))... 289 | = [values zeros(1,2*(order+1))]... 290 | - t_vector(1)*[t_vector(1)*values t_vector(2)*values t_vector(3)*values]; 291 | b(1) = corridor_width +... 292 | rix+t_vector(1)*(-rix*t_vector(1) -riy*t_vector(2) -riz*t_vector(3)); 293 | c(2,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1))... 294 | = -c(1,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1)); 295 | b(2) = corridor_width +... 296 | -rix-t_vector(1)*(-rix*t_vector(1) -riy*t_vector(2) -riz*t_vector(3)); 297 | %y 298 | c(3,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1))... 299 | = [zeros(1,order+1) values zeros(1,order+1)]... 300 | - t_vector(2)*[t_vector(1)*values t_vector(2)*values t_vector(3)*values]; 301 | b(3) = corridor_width +... 302 | riy+t_vector(2)*(-rix*t_vector(1) -riy*t_vector(2) -riz*t_vector(3)); 303 | c(4,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1))... 304 | = -c(3,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1)); 305 | b(4) = corridor_width +... 306 | -riy-t_vector(2)*(-rix*t_vector(1) -riy*t_vector(2) -riz*t_vector(3)); 307 | %z 308 | c(5,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1))... 309 | = [zeros(1,2*(order+1)) values]... 310 | - t_vector(3)*[t_vector(1)*values t_vector(2)*values t_vector(3)*values]; 311 | b(5) = corridor_width +... 312 | riz+t_vector(3)*(-rix*t_vector(1) -riy*t_vector(2) -riz*t_vector(3)); 313 | c(6,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1))... 314 | = -c(5,(corridor_position(1)-1)*n*(order+1)+0*(order+1)+1:(corridor_position(1)-1)*n*(order+1)+3*(order+1)); 315 | b(6) = corridor_width +... 316 | -riz-t_vector(3)*(-rix*t_vector(1) -riy*t_vector(2) -riz*t_vector(3)); 317 | 318 | C3 = [C3; c]; 319 | b3 = [b3; b]; 320 | end 321 | 322 | C = [C1; C2]; 323 | b = [b1; b2]; 324 | end -------------------------------------------------------------------------------- /constraintData.m: -------------------------------------------------------------------------------- 1 | %Only for the quadrotor system 2 | 3 | %Position constraints 4 | constraintData_r = zeros(m,k_r,3); 5 | 6 | %velocity 7 | if(k_r>=1) 8 | constraintData_r(1,1,1:3) = 0; %At starting position 9 | constraintData_r(2:m,1,1:2) = eps; %x,y velocities 10 | constraintData_r(2:m,1,3) = eps; %z velocity 11 | end 12 | %acceleration 13 | if(k_r>=2) 14 | constraintData_r(1,2,3) = 0; %At starting position 15 | constraintData_r(2:m,2,1:2) = eps; %x,y accelerations 16 | constraintData_r(2:m,2,3) = eps; %z acceleration 17 | end 18 | %jerk 19 | if(k_r>=3) 20 | %all zeros 21 | end 22 | %snap 23 | if(k_r>=4) 24 | %all zeros 25 | end 26 | 27 | %Yaw constraints 28 | constraintData_psi = zeros(m,k_psi,1); 29 | 30 | %velocity 31 | if(k_psi>=1) 32 | constraintData_psi(1,1,1) = 0; %At starting position 33 | end 34 | %acceleration 35 | if(k_psi>=2) 36 | %all zeros 37 | end 38 | %jerk 39 | if(k_psi>=3) 40 | %all zeros 41 | end 42 | %snap 43 | if(k_psi>=4) 44 | %all zeros 45 | end 46 | -------------------------------------------------------------------------------- /drawSolution.m: -------------------------------------------------------------------------------- 1 | x_trajec = []; 2 | y_trajec = []; 3 | z_trajec = []; 4 | psi_trajec = []; 5 | time = []; 6 | 7 | x_trajec_corridor = []; 8 | y_trajec_corridor = []; 9 | z_trajec_corridor = []; 10 | psi_trajec_corridor = []; 11 | 12 | for i=1:m 13 | x_trajec = [x_trajec polyval(solution((i-1)*n*(order+1)+1+0*(order+1):(i-1)*n*(order+1)+(order+1)+0*(order+1)),t(i):0.01:t(i+1))]; 14 | y_trajec = [y_trajec polyval(solution((i-1)*n*(order+1)+1+1*(order+1):(i-1)*n*(order+1)+(order+1)+1*(order+1)),t(i):0.01:t(i+1))]; 15 | z_trajec = [z_trajec polyval(solution((i-1)*n*(order+1)+1+2*(order+1):(i-1)*n*(order+1)+(order+1)+2*(order+1)),t(i):0.01:t(i+1))]; 16 | psi_trajec = [psi_trajec polyval(solution((i-1)*n*(order+1)+1+3*(order+1):(i-1)*n*(order+1)+(order+1)+3*(order+1)),t(i):0.01:t(i+1))]; 17 | time = [time t(i):0.01:t(i+1)]; 18 | 19 | x_trajec_corridor = [x_trajec_corridor polyval(solution_corridor((i-1)*n*(order+1)+1+0*(order+1):(i-1)*n*(order+1)+(order+1)+0*(order+1)),t(i):0.01:t(i+1))]; 20 | y_trajec_corridor = [y_trajec_corridor polyval(solution_corridor((i-1)*n*(order+1)+1+1*(order+1):(i-1)*n*(order+1)+(order+1)+1*(order+1)),t(i):0.01:t(i+1))]; 21 | z_trajec_corridor = [z_trajec_corridor polyval(solution_corridor((i-1)*n*(order+1)+1+2*(order+1):(i-1)*n*(order+1)+(order+1)+2*(order+1)),t(i):0.01:t(i+1))]; 22 | psi_trajec_corridor = [psi_trajec_corridor polyval(solution_corridor((i-1)*n*(order+1)+1+3*(order+1):(i-1)*n*(order+1)+(order+1)+3*(order+1)),t(i):0.01:t(i+1))]; 23 | end 24 | 25 | figure(1); 26 | subplot(2,2,1); 27 | plot(x_trajec,y_trajec,'-k'); 28 | plot(x_trajec_corridor,y_trajec_corridor,'-r'); 29 | subplot(2,2,[3 4]); 30 | plot3(x_trajec,y_trajec,z_trajec,'-k'); 31 | plot3(x_trajec_corridor,y_trajec_corridor,z_trajec_corridor,'-r'); 32 | 33 | figure(2); 34 | subplot(4,1,1); plot(time,x_trajec,'-k'); ylabel('x'); hold on; 35 | plot(time, x_trajec_corridor,'-r'); 36 | subplot(4,1,2); plot(time,y_trajec,'-k'); ylabel('y'); hold on; 37 | plot(time, y_trajec_corridor,'-r'); 38 | subplot(4,1,3); plot(time,z_trajec,'-k'); ylabel('z'); hold on; 39 | plot(time, z_trajec_corridor,'-r'); 40 | subplot(4,1,4); plot(time,psi_trajec,'-k'); xlabel('time'); ylabel('\psi'); hold on; 41 | plot(time, psi_trajec_corridor,'-r'); 42 | 43 | for i=1:size(keyframe,2) 44 | subplot(4,1,1); plot(t(i),keyframe(1,i),'or','MarkerFaceColor','r'); 45 | subplot(4,1,2); plot(t(i),keyframe(2,i),'or','MarkerFaceColor','r'); 46 | subplot(4,1,3); plot(t(i),keyframe(3,i),'or','MarkerFaceColor','r'); 47 | subplot(4,1,4); plot(t(i),keyframe(4,i),'or','MarkerFaceColor','r'); 48 | end -------------------------------------------------------------------------------- /keyframe.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Clark-Youngdong-Son/minSnapTrajec_matlab/26240f6f1a9372827f192f8ad63c1a924ca8614e/keyframe.mat -------------------------------------------------------------------------------- /keyframe_selection.m: -------------------------------------------------------------------------------- 1 | %% Keyframe selection from mouse point 2 | 3 | %Yaw is fixed to zero for all keyframes. 4 | 5 | 6 | %Plots initialization 7 | figure(1); 8 | subplot(2,2,1); %x-y view 9 | grid on; hold on; 10 | xlabel('xy plane'); 11 | axis([-5 20 -12.5 12.5]); 12 | 13 | subplot(2,2,2); %altitude view 14 | set( gca, 'XTick', -1:1:1 ); 15 | set( gca, 'XTickLabel', {'','',''} ); 16 | grid on; hold on; 17 | xlabel('altitude'); 18 | axis([-1 1 0 5]); 19 | 20 | subplot(2,2,[3 4]); %3-dimensional view 21 | grid on; hold on; 22 | xlabel('3D view'); 23 | axis([-5 20 -12.5 12.5 0 5]); 24 | 25 | %Get data from mouse input 26 | global count keyframe; 27 | count = 1; 28 | keyframe = zeros(4,m); 29 | keyframe(:,1) = zeros(4,1); %initial position and yaw 30 | 31 | %xy selection 32 | subplot(2,2,1); 33 | plot(keyframe(1,count), keyframe(2,count), 'ro','MarkerFaceColor', 'r'); 34 | text(keyframe(1,count)+0.6, keyframe(2,count)+0.6, num2str(count)); 35 | set(gcf, 'WindowButtonMotionFcn', @mouseMove1); 36 | set(gcf, 'WindowButtonDownFcn', @mouseClick1); 37 | while(count~=m) 38 | drawnow; 39 | end 40 | title(gca,'Done. Select the desired altitudes'); 41 | 42 | %altitude selection 43 | count = 1; 44 | subplot(2,2,2); 45 | plot([-1 1],[0 0], 'r-', 'LineWidth', 2); 46 | text(0.6, 0.6, num2str(count)); 47 | set(gcf, 'WindowButtonMotionFcn', @mouseMove2); 48 | set(gcf, 'WindowButtonDownFcn', @mouseClick2); 49 | while(count~=m) 50 | drawnow; 51 | end 52 | title(gca,'All selections done'); 53 | 54 | %3D view 55 | subplot(2,2,[3 4]); 56 | for i=1:m 57 | plot3(keyframe(1,i),keyframe(2,i),keyframe(3,i),'-ro','MarkerFaceColor','r'); 58 | text(keyframe(1,i)+0.4, keyframe(2,i)+0.4, keyframe(3,i)+0.4, num2str(i)); 59 | end 60 | view([-30.4000 45.2000]); 61 | drawnow; -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | %% main 2 | clear all; close all; 3 | 4 | parameters; %parameters initialization 5 | 6 | if(keyframe_selection_flag) 7 | keyframe_selection; 8 | else 9 | load('keyframe.mat','keyframe'); 10 | end 11 | 12 | QP_trajec_generation; 13 | 14 | drawSolution; -------------------------------------------------------------------------------- /mouseClick1.m: -------------------------------------------------------------------------------- 1 | function mouseClick1(object, eventdata) 2 | global count m keyframe; 3 | 4 | if(count < m) 5 | count = count + 1; 6 | C = get(gca, 'CurrentPoint'); 7 | keyframe(1,count) = C(1,1); %x 8 | keyframe(2,count) = C(1,2); %y 9 | plot(keyframe(1,count), keyframe(2,count), 'ro','MarkerFaceColor', 'r'); 10 | text(keyframe(1,count)+0.6, keyframe(2,count)+0.6, num2str(count)); 11 | end 12 | end -------------------------------------------------------------------------------- /mouseClick2.m: -------------------------------------------------------------------------------- 1 | function mouseClick2(object, eventdata) 2 | global count m keyframe; 3 | 4 | if(count < m) 5 | count = count + 1; 6 | C = get(gca, 'CurrentPoint'); 7 | keyframe(3,count) = C(1,2); %z 8 | if(count == m) 9 | keyframe(:,count+1) = keyframe(:,1); %Return to the original position 10 | end 11 | plot([-1 1],[keyframe(3,count) keyframe(3,count)], 'r-', 'LineWidth',2); 12 | text(0.6, keyframe(3,count)+0.6, num2str(count)); 13 | end 14 | end -------------------------------------------------------------------------------- /mouseMove1.m: -------------------------------------------------------------------------------- 1 | function mouseMove1(object, eventdata) 2 | global count m; 3 | if(count~=m) 4 | C = get(gca, 'CurrentPoint'); 5 | title(gca,['(X,Y) = (', num2str(C(1,1),2), ', ',num2str(C(1,2),2), ')']); 6 | end 7 | end -------------------------------------------------------------------------------- /mouseMove2.m: -------------------------------------------------------------------------------- 1 | function mouseMove2(object, eventdata) 2 | global count m; 3 | if(count~=m) 4 | C = get(gca, 'CurrentPoint'); 5 | title(gca,['Altitude = (', num2str(C(1,2),2), ')']); 6 | end 7 | end -------------------------------------------------------------------------------- /parameters.m: -------------------------------------------------------------------------------- 1 | %% Parameters 2 | %System 3 | m_q = 4; %mass of a quadrotor 4 | I_q = diag([0.3 0.3 0.3]); %moment of inertia of a quadrotor 5 | g = 9.81; %gravitational acceleration 6 | 7 | %Trajectory 8 | global m; 9 | n = 4; %number of flat outputs (x, y, z, psi) 10 | t_f = 10; %final time of the simulation 11 | 12 | keyframe_selection_flag = true; %true : select manually, false : use the given test data 13 | if(keyframe_selection_flag) 14 | m = 5; %number of keyframes 15 | end 16 | 17 | order = 6; %order of polynomial functions 18 | 19 | time_interval_selection_flag = true; %true : fixed time interval, false : optimal time interval 20 | if(time_interval_selection_flag) 21 | t = linspace(0,t_f,m+1); 22 | end 23 | 24 | n_intermediate = 5; 25 | corridor_width = 0.05; 26 | corridor_position = [3 4]; %from the i keyframe to the j keyframe --------------------------------------------------------------------------------