└── workshop_github ├── Codes_casadi_v3_4_5 ├── MHE_code │ ├── Draw_MPC_point_stabilization_v1.m │ ├── MHE_Robot_PS_mul_shooting_v1.m │ ├── MHE_Robot_PS_mul_shooting_v2.m │ └── shift.m ├── MPC_code │ ├── Draw_MPC_PS_Obstacles.m │ ├── Draw_MPC_point_stabilization_v1.m │ ├── Draw_MPC_tracking_v1.m │ ├── Sim_1_MPC_Robot_PS_sing_shooting.m │ ├── Sim_2_MPC_R_PS_mul_shooting_RK.m │ ├── Sim_2_MPC_Robot_PS_mul_shooting.m │ ├── Sim_3_MPC_Robot_PS_obs_avoid_mul_sh.m │ ├── Sim_4_MPC_Robot_tracking_mul_shooting.m │ └── shift.m ├── motivation_example_1.m ├── motivation_example_2.m ├── motivation_example_3.m └── motivation_example_3.xlsx ├── Codes_casadi_v3_5_5 ├── MHE_code │ ├── Draw_MPC_point_stabilization_v1.m │ ├── MHE_Robot_PS_mul_shooting_v1.m │ ├── MHE_Robot_PS_mul_shooting_v2.m │ └── shift.m ├── MPC_code │ ├── Draw_MPC_PS_Obstacles.m │ ├── Draw_MPC_point_stabilization_v1.m │ ├── Draw_MPC_tracking_v1.m │ ├── Sim_1_MPC_Robot_PS_sing_shooting.m │ ├── Sim_2_MPC_R_PS_mul_shooting_RK.m │ ├── Sim_2_MPC_Robot_PS_mul_shooting.m │ ├── Sim_3_MPC_Robot_PS_obs_avoid_mul_sh.m │ ├── Sim_4_MPC_Robot_tracking_mul_shooting.m │ ├── exp.avi │ └── shift.m ├── motivation_example_1.m ├── motivation_example_2.m ├── motivation_example_3.m └── motivation_example_3.xlsx ├── MPC_MHE_slides.pdf ├── MPC_tracking.pdf ├── Python_Implementation ├── animation1612211110.9228647.gif ├── mpc_code.py ├── readme.md └── simulation_code.py ├── ReadMe.txt └── model_simulation_using_RK.pdf /workshop_github/Codes_casadi_v3_4_5/MHE_code/Draw_MPC_point_stabilization_v1.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 2 | 3 | 4 | set(0,'DefaultAxesFontName', 'Times New Roman') 5 | set(0,'DefaultAxesFontSize', 12) 6 | 7 | line_width = 1.5; 8 | fontsize_labels = 14; 9 | 10 | %-------------------------------------------------------------------------- 11 | %-----------------------Simulate robots ----------------------------------- 12 | %-------------------------------------------------------------------------- 13 | x_r_1 = []; 14 | y_r_1 = []; 15 | 16 | 17 | 18 | r = rob_diam/2; % obstacle radius 19 | ang=0:0.005:2*pi; 20 | xp=r*cos(ang); 21 | yp=r*sin(ang); 22 | 23 | figure(500) 24 | % Animate the robot motion 25 | %figure;%('Position',[200 200 1280 720]); 26 | set(gcf,'PaperPositionMode','auto') 27 | set(gcf, 'Color', 'w'); 28 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 29 | 30 | tic 31 | for k = 1:size(xx,2) 32 | h_t = 0.14; w_t=0.09; % triangle parameters 33 | 34 | x1 = xs(1); y1 = xs(2); th1 = xs(3); 35 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 36 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 37 | fill(x1_tri, y1_tri, 'g'); % plot reference state 38 | hold on; 39 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 40 | x_r_1 = [x_r_1 x1]; 41 | y_r_1 = [y_r_1 y1]; 42 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 43 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 44 | 45 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 46 | if k < size(xx,2) % plot prediction 47 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 48 | end 49 | 50 | fill(x1_tri, y1_tri, 'r'); % plot robot position 51 | plot(x1+xp,y1+yp,'--r'); % plot robot circle 52 | 53 | 54 | hold off 55 | %figure(500) 56 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 57 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 58 | axis([-0.2 1.8 -0.2 1.8]) 59 | pause(0.0) 60 | box on; 61 | grid on 62 | %aviobj = addframe(aviobj,gcf); 63 | drawnow 64 | % for video generation 65 | F(k) = getframe(gcf); % to get the current frame 66 | end 67 | toc 68 | close(gcf) 69 | %viobj = close(aviobj) 70 | %video = VideoWriter('exp.avi','Uncompressed AVI'); 71 | 72 | % video = VideoWriter('exp.avi','Motion JPEG AVI'); 73 | % video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 74 | % open(video) 75 | % writeVideo(video,F) 76 | % close (video) 77 | 78 | figure 79 | subplot(211) 80 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.35 0.75]) 81 | ylabel('v (rad/s)') 82 | grid on 83 | subplot(212) 84 | stairs(t,u_cl(:,2),'r','linewidth',1.5); axis([0 t(end) -0.85 0.85]) 85 | xlabel('time (seconds)') 86 | ylabel('\omega (rad/s)') 87 | grid on 88 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MHE_code/MHE_Robot_PS_mul_shooting_v1.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.1.1 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 10; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include at the initial state of the robot and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % make the decision variable one column vector 49 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 50 | 51 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 52 | 53 | opts = struct; 54 | opts.ipopt.max_iter = 2000; 55 | opts.ipopt.print_level =0;%0,3 56 | opts.print_time = 0; 57 | opts.ipopt.acceptable_tol =1e-8; 58 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 59 | 60 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 61 | 62 | 63 | args = struct; 64 | 65 | args.lbg(1:3*(N+1)) = 0; 66 | args.ubg(1:3*(N+1)) = 0; 67 | 68 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 69 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 70 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 71 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 72 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 73 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 74 | 75 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 76 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 77 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 78 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 79 | %---------------------------------------------- 80 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 81 | 82 | 83 | % THE SIMULATION LOOP SHOULD START FROM HERE 84 | %------------------------------------------- 85 | t0 = 0; 86 | x0 = [0.1 ; 0.1 ; 0.0]; % initial condition. 87 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 88 | 89 | xx(:,1) = x0; % xx contains the history of states 90 | t(1) = t0; 91 | 92 | u0 = zeros(N,2); % two control inputs for each robot 93 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 94 | 95 | sim_tim = 20; % total sampling times 96 | 97 | % Start MPC 98 | mpciter = 0; 99 | xx1 = []; 100 | u_cl=[]; 101 | 102 | % the main simulaton loop... it works as long as the error is greater 103 | % than 10^-6 and the number of mpc steps is less than its maximum 104 | % value. 105 | tic 106 | while(norm((x0-xs),2) > 0.05 && mpciter < sim_tim / T) 107 | args.p = [x0;xs]; % set the values of the parameters vector 108 | % initial value of the optimization variables 109 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 110 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 111 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 112 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 113 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 114 | u_cl= [u_cl ; u(1,:)]; 115 | t(mpciter+1) = t0; 116 | % Apply the control and shift the solution 117 | [t0, x0, u0] = shift(T, t0, x0, u,f); 118 | xx(:,mpciter+2) = x0; 119 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 120 | % Shift trajectory to initialize the next step 121 | X0 = [X0(2:end,:);X0(end,:)]; 122 | mpciter 123 | mpciter = mpciter + 1; 124 | end; 125 | toc 126 | 127 | ss_error = norm((x0-xs),2) 128 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 129 | %----------------------------------------- 130 | %----------------------------------------- 131 | %----------------------------------------- 132 | % Start MHE implementation from here 133 | %----------------------------------------- 134 | %----------------------------------------- 135 | %----------------------------------------- 136 | % plot the ground truth 137 | figure(1) 138 | subplot(311) 139 | plot(t,xx(1,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 140 | ylabel('x (m)') 141 | grid on 142 | subplot(312) 143 | plot(t,xx(2,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 144 | ylabel('y (m)') 145 | grid on 146 | subplot(313) 147 | plot(t,xx(3,1:end-1),'b','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 148 | xlabel('time (seconds)') 149 | ylabel('\theta (rad)') 150 | grid on 151 | 152 | % Synthesize the measurments 153 | con_cov = diag([0.005 deg2rad(2)]).^2; 154 | meas_cov = diag([0.1 deg2rad(2)]).^2; 155 | 156 | r = []; 157 | alpha = []; 158 | for k = 1: length(xx(1,:))-1 159 | r = [r; sqrt(xx(1,k)^2+xx(2,k)^2) + sqrt(meas_cov(1,1))*randn(1)]; 160 | alpha = [alpha; atan(xx(2,k)/xx(1,k)) + sqrt(meas_cov(2,2))*randn(1)]; 161 | end 162 | y_measurements = [ r , alpha ]; 163 | 164 | % Plot the cartesian coordinates from the measurements used 165 | figure(1) 166 | subplot(311) 167 | plot(t,r.*cos(alpha),'r','linewidth',1.5); hold on 168 | grid on 169 | legend('Ground Truth','Measurement') 170 | subplot(312) 171 | plot(t,r.*sin(alpha),'r','linewidth',1.5); hold on 172 | grid on 173 | 174 | % plot the ground truth mesurements VS the noisy measurements 175 | figure(2) 176 | subplot(211) 177 | plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on 178 | plot(t,r,'r','linewidth',1.5); axis([0 t(end) -0.2 3]); hold on 179 | ylabel('Range: [ r (m) ]') 180 | grid on 181 | legend('Ground Truth','Measurement') 182 | subplot(212) 183 | plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on 184 | plot(t,alpha,'r','linewidth',1.5); axis([0 t(end) 0.2 1]); hold on 185 | ylabel('Bearing: [ \alpha (rad) ]') 186 | grid on 187 | 188 | % The following two matrices contain what we know about the system, i.e. 189 | % the nominal control actions applied (measured) and the range and bearing 190 | % measurements. 191 | %------------------------------------------------------------------------- 192 | u_cl; 193 | y_measurements; 194 | 195 | % Let's now formulate our MHE problem 196 | %------------------------------------ 197 | T = 0.2; %[s] 198 | N_MHE = size(y_measurements,1)-1; % Estimation horizon 199 | 200 | v_max = 0.6; v_min = -v_max; 201 | omega_max = pi/4; omega_min = -omega_max; 202 | 203 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 204 | states = [x;y;theta]; n_states = length(states); 205 | v = SX.sym('v'); omega = SX.sym('omega'); 206 | controls = [v;omega]; n_controls = length(controls); 207 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 208 | f = Function('f',{states,controls},{rhs}); % MOTION MODEL 209 | 210 | r = SX.sym('r'); alpha = SX.sym('alpha'); % range and bearing 211 | measurement_rhs = [sqrt(x^2+y^2); atan(y/x)]; 212 | h = Function('h',{states},{measurement_rhs}); % MEASUREMENT MODEL 213 | %y_tilde = [r;alpha]; 214 | 215 | % Decision variables 216 | U = SX.sym('U',n_controls,N_MHE); %(controls) 217 | X = SX.sym('X',n_states,(N_MHE+1)); %(states) [remember multiple shooting] 218 | 219 | P = SX.sym('P', 2 , (N_MHE+1) + N_MHE); 220 | % parameters (include r and alpha measurements as well as controls measurements) 221 | 222 | V = inv(sqrt(meas_cov)); % weighing matrices (output) y_tilde - y 223 | W = inv(sqrt(con_cov)); % weighing matrices (input) u_tilde - u 224 | 225 | obj = 0; % Objective function 226 | g = []; % constraints vector 227 | for k = 1:N_MHE+1 228 | st = X(:,k); 229 | h_x = h(st); 230 | y_tilde = P(:,k); 231 | obj = obj+ (y_tilde-h_x)' * V * (y_tilde-h_x); % calculate obj 232 | end 233 | 234 | for k = 1:N_MHE 235 | con = U(:,k); 236 | u_tilde = P(:,N_MHE+ k); 237 | obj = obj+ (u_tilde-con)' * W * (u_tilde-con); % calculate obj 238 | end 239 | 240 | % multiple shooting constraints 241 | for k = 1:N_MHE 242 | st = X(:,k); con = U(:,k); 243 | st_next = X(:,k+1); 244 | f_value = f(st,con); 245 | st_next_euler = st+ (T*f_value); 246 | g = [g;st_next-st_next_euler]; % compute constraints 247 | end 248 | 249 | 250 | % make the decision variable one column vector 251 | OPT_variables = [reshape(X,3*(N_MHE+1),1);reshape(U,2*N_MHE,1)]; 252 | 253 | nlp_mhe = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 254 | 255 | opts = struct; 256 | opts.ipopt.max_iter = 2000; 257 | opts.ipopt.print_level =0;%0,3 258 | opts.print_time = 0; 259 | opts.ipopt.acceptable_tol =1e-8; 260 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 261 | 262 | solver = nlpsol('solver', 'ipopt', nlp_mhe,opts); 263 | 264 | args = struct; 265 | 266 | args.lbg(1:3*(N_MHE)) = 0; % equality constraints 267 | args.ubg(1:3*(N_MHE)) = 0; % equality constraints 268 | 269 | args.lbx(1:3:3*(N_MHE+1),1) = -2; %state x lower bound 270 | args.ubx(1:3:3*(N_MHE+1),1) = 2; %state x upper bound 271 | args.lbx(2:3:3*(N_MHE+1),1) = -2; %state y lower bound 272 | args.ubx(2:3:3*(N_MHE+1),1) = 2; %state y upper bound 273 | args.lbx(3:3:3*(N_MHE+1),1) = -pi/2; %state theta lower bound 274 | args.ubx(3:3:3*(N_MHE+1),1) = pi/2; %state theta upper bound 275 | 276 | args.lbx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_min; %v lower bound 277 | args.ubx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_max; %v upper bound 278 | args.lbx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_min; %omega lower bound 279 | args.ubx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_max; %omega upper bound 280 | %---------------------------------------------- 281 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 282 | 283 | % MHE Simulation 284 | %------------------------------------------ 285 | U0 = zeros(N_MHE,2); % two control inputs for each robot 286 | X0 = zeros(N_MHE+1,3); % initialization of the states decision variables 287 | 288 | U0 = u_cl(1:N_MHE,:); % initialize the control actions by the measured 289 | % initialize the states from the measured range and bearing 290 | X0(:,1:2) = [y_measurements(1:N_MHE+1,1).*cos(y_measurements(1:N_MHE+1,2)),... 291 | y_measurements(1:N_MHE+1,1).*sin(y_measurements(1:N_MHE+1,2))]; 292 | 293 | k=1; 294 | % Get the measurements window and put it as parameters in p 295 | args.p = [y_measurements(k:k+N_MHE,:)',u_cl(k:k+N_MHE-1,:)']; 296 | % initial value of the optimization variables 297 | args.x0 = [reshape(X0',3*(N_MHE+1),1);reshape(U0',2*N_MHE,1)]; 298 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 299 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 300 | U_sol = reshape(full(sol.x(3*(N_MHE+1)+1:end))',2,N_MHE)'; % get controls only from the solution 301 | X_sol = reshape(full(sol.x(1:3*(N_MHE+1)))',3,N_MHE+1)'; % get solution TRAJECTORY 302 | 303 | figure(1) 304 | subplot(311) 305 | plot(t,X_sol(:,1),'g','linewidth',1.5); hold on 306 | legend('Ground Truth','Measurement','MHE') 307 | subplot(312) 308 | plot(t,X_sol(:,2),'g','linewidth',1.5); hold on 309 | subplot(313) 310 | plot(t,X_sol(:,3),'g','linewidth',1.5); hold on 311 | 312 | 313 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MHE_code/MHE_Robot_PS_mul_shooting_v2.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.1.1 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 10; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include at the initial state of the robot and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % make the decision variable one column vector 49 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 50 | 51 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 52 | 53 | opts = struct; 54 | opts.ipopt.max_iter = 2000; 55 | opts.ipopt.print_level =0;%0,3 56 | opts.print_time = 0; 57 | opts.ipopt.acceptable_tol =1e-8; 58 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 59 | 60 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 61 | 62 | 63 | args = struct; 64 | 65 | args.lbg(1:3*(N+1)) = 0; 66 | args.ubg(1:3*(N+1)) = 0; 67 | 68 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 69 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 70 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 71 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 72 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 73 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 74 | 75 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 76 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 77 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 78 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 79 | %---------------------------------------------- 80 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 81 | 82 | 83 | % THE SIMULATION LOOP SHOULD START FROM HERE 84 | %------------------------------------------- 85 | t0 = 0; 86 | x0 = [0.1 ; 0.1 ; 0.0]; % initial condition. 87 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 88 | 89 | xx(:,1) = x0; % xx contains the history of states 90 | t(1) = t0; 91 | 92 | u0 = zeros(N,2); % two control inputs for each robot 93 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 94 | 95 | sim_tim = 20; % total sampling times 96 | 97 | % Start MPC 98 | mpciter = 0; 99 | xx1 = []; 100 | u_cl=[]; 101 | 102 | % the main simulaton loop... it works as long as the error is greater 103 | % than 10^-6 and the number of mpc steps is less than its maximum 104 | % value. 105 | tic 106 | while(norm((x0-xs),2) > 0.05 && mpciter < sim_tim / T) 107 | args.p = [x0;xs]; % set the values of the parameters vector 108 | % initial value of the optimization variables 109 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 110 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 111 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 112 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 113 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 114 | u_cl= [u_cl ; u(1,:)]; 115 | t(mpciter+1) = t0; 116 | % Apply the control and shift the solution 117 | [t0, x0, u0] = shift(T, t0, x0, u,f); 118 | xx(:,mpciter+2) = x0; 119 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 120 | % Shift trajectory to initialize the next step 121 | X0 = [X0(2:end,:);X0(end,:)]; 122 | mpciter 123 | mpciter = mpciter + 1; 124 | end; 125 | toc 126 | 127 | ss_error = norm((x0-xs),2) 128 | %Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 129 | %----------------------------------------- 130 | %----------------------------------------- 131 | %----------------------------------------- 132 | % Start MHE implementation from here 133 | %----------------------------------------- 134 | %----------------------------------------- 135 | %----------------------------------------- 136 | % plot the ground truth 137 | figure(1) 138 | subplot(311) 139 | plot(t,xx(1,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 140 | ylabel('x (m)') 141 | grid on 142 | subplot(312) 143 | plot(t,xx(2,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 144 | ylabel('y (m)') 145 | grid on 146 | subplot(313) 147 | plot(t,xx(3,1:end-1),'b','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 148 | xlabel('time (seconds)') 149 | ylabel('\theta (rad)') 150 | grid on 151 | 152 | % Synthesize the measurments 153 | con_cov = diag([0.005 deg2rad(2)]).^2; 154 | meas_cov = diag([0.1 deg2rad(2)]).^2; 155 | 156 | r = []; 157 | alpha = []; 158 | for k = 1: length(xx(1,:))-1 159 | r = [r; sqrt(xx(1,k)^2+xx(2,k)^2) + sqrt(meas_cov(1,1))*randn(1)]; 160 | alpha = [alpha; atan(xx(2,k)/xx(1,k)) + sqrt(meas_cov(2,2))*randn(1)]; 161 | end 162 | y_measurements = [ r , alpha ]; 163 | 164 | % Plot the cartesian coordinates from the measurements used 165 | figure(1) 166 | subplot(311) 167 | plot(t,r.*cos(alpha),'r','linewidth',1.5); hold on 168 | grid on 169 | legend('Ground Truth','Measurement') 170 | subplot(312) 171 | plot(t,r.*sin(alpha),'r','linewidth',1.5); hold on 172 | grid on 173 | 174 | % plot the ground truth mesurements VS the noisy measurements 175 | figure(2) 176 | subplot(211) 177 | plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on 178 | plot(t,r,'r','linewidth',1.5); axis([0 t(end) -0.2 3]); hold on 179 | ylabel('Range: [ r (m) ]') 180 | grid on 181 | legend('Ground Truth','Measurement') 182 | subplot(212) 183 | plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on 184 | plot(t,alpha,'r','linewidth',1.5); axis([0 t(end) 0.2 1]); hold on 185 | ylabel('Bearing: [ \alpha (rad) ]') 186 | grid on 187 | 188 | % The following two matrices contain what we know about the system, i.e. 189 | % the nominal control actions applied (measured) and the range and bearing 190 | % measurements. 191 | %------------------------------------------------------------------------- 192 | u_cl; 193 | y_measurements; 194 | 195 | % Let's now formulate our MHE problem 196 | %------------------------------------ 197 | T = 0.2; %[s] 198 | N_MHE = 6; % prediction horizon 199 | 200 | v_max = 0.6; v_min = -v_max; 201 | omega_max = pi/4; omega_min = -omega_max; 202 | 203 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 204 | states = [x;y;theta]; n_states = length(states); 205 | v = SX.sym('v'); omega = SX.sym('omega'); 206 | controls = [v;omega]; n_controls = length(controls); 207 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 208 | f = Function('f',{states,controls},{rhs}); % MOTION MODEL 209 | 210 | r = SX.sym('r'); alpha = SX.sym('alpha'); % range and bearing 211 | measurement_rhs = [sqrt(x^2+y^2); atan(y/x)]; 212 | h = Function('h',{states},{measurement_rhs}); % MEASUREMENT MODEL 213 | %y_tilde = [r;alpha]; 214 | 215 | % Decision variables 216 | U = SX.sym('U',n_controls,N_MHE); %(controls) 217 | X = SX.sym('X',n_states,(N_MHE+1)); %(states) [remember multiple shooting] 218 | 219 | P = SX.sym('P', 2 , N_MHE + (N_MHE+1)); 220 | % parameters (include r and alpha measurements as well as controls measurements) 221 | 222 | V = inv(sqrt(meas_cov)); % weighing matrices (output) y_tilde - y 223 | W = inv(sqrt(con_cov)); % weighing matrices (input) u_tilde - u 224 | 225 | obj = 0; % Objective function 226 | g = []; % constraints vector 227 | for k = 1:N_MHE+1 228 | st = X(:,k); 229 | h_x = h(st); 230 | y_tilde = P(:,k); 231 | obj = obj+ (y_tilde-h_x)' * V * (y_tilde-h_x); % calculate obj 232 | end 233 | 234 | for k = 1:N_MHE 235 | con = U(:,k); 236 | u_tilde = P(:,N_MHE+ k); 237 | obj = obj+ (u_tilde-con)' * W * (u_tilde-con); % calculate obj 238 | end 239 | 240 | % multiple shooting constraints 241 | for k = 1:N_MHE 242 | st = X(:,k); con = U(:,k); 243 | st_next = X(:,k+1); 244 | f_value = f(st,con); 245 | st_next_euler = st+ (T*f_value); 246 | g = [g;st_next-st_next_euler]; % compute constraints 247 | end 248 | 249 | 250 | % make the decision variable one column vector 251 | OPT_variables = [reshape(X,3*(N_MHE+1),1);reshape(U,2*N_MHE,1)]; 252 | 253 | nlp_mhe = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 254 | 255 | opts = struct; 256 | opts.ipopt.max_iter = 2000; 257 | opts.ipopt.print_level =0;%0,3 258 | opts.print_time = 0; 259 | opts.ipopt.acceptable_tol =1e-8; 260 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 261 | 262 | solver = nlpsol('solver', 'ipopt', nlp_mhe,opts); 263 | 264 | 265 | args = struct; 266 | 267 | args.lbg(1:3*(N_MHE)) = 0; 268 | args.ubg(1:3*(N_MHE)) = 0; 269 | 270 | args.lbx(1:3:3*(N_MHE+1),1) = -2; %state x lower bound 271 | args.ubx(1:3:3*(N_MHE+1),1) = 2; %state x upper bound 272 | args.lbx(2:3:3*(N_MHE+1),1) = -2; %state y lower bound 273 | args.ubx(2:3:3*(N_MHE+1),1) = 2; %state y upper bound 274 | args.lbx(3:3:3*(N_MHE+1),1) = -pi/2; %state theta lower bound 275 | args.ubx(3:3:3*(N_MHE+1),1) = pi/2; %state theta upper bound 276 | 277 | args.lbx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_min; %v lower bound 278 | args.ubx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_max; %v upper bound 279 | args.lbx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_min; %omega lower bound 280 | args.ubx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_max; %omega upper bound 281 | %---------------------------------------------- 282 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 283 | 284 | % MHE Simulation loop starts here 285 | %------------------------------------------ 286 | X_estimate = []; % X_estimate contains the MHE estimate of the states 287 | U_estimate = []; % U_estimate contains the MHE estimate of the controls 288 | 289 | U0 = zeros(N_MHE,2); % two control inputs for each robot 290 | X0 = zeros(N_MHE+1,3); % initialization of the states decision variables 291 | 292 | % Start MHE 293 | mheiter = 0; 294 | 295 | U0 = u_cl(1:N_MHE,:); % initialize the control actions by the measured 296 | % initialize the states from the measured range and bearing 297 | X0(:,1:2) = [y_measurements(1:N_MHE+1,1).*cos(y_measurements(1:N_MHE+1,2)),... 298 | y_measurements(1:N_MHE+1,1).*sin(y_measurements(1:N_MHE+1,2))]; 299 | 300 | tic 301 | for k = 1: size(y_measurements,1) - (N_MHE) 302 | mheiter = k 303 | % Get the measurements window and put it as parameters in p 304 | args.p = [y_measurements(k:k+N_MHE,:)',u_cl(k:k+N_MHE-1,:)']; 305 | % initial value of the optimization variables 306 | args.x0 = [reshape(X0',3*(N_MHE+1),1);reshape(U0',2*N_MHE,1)]; 307 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 308 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 309 | U_sol = reshape(full(sol.x(3*(N_MHE+1)+1:end))',2,N_MHE)'; % get controls only from the solution 310 | X_sol = reshape(full(sol.x(1:3*(N_MHE+1)))',3,N_MHE+1)'; % get solution TRAJECTORY 311 | X_estimate = [X_estimate;X_sol(N_MHE+1,:)]; 312 | U_estimate = [U_estimate;U_sol(N_MHE,:)]; 313 | 314 | % Shift trajectory to initialize the next step 315 | X0 = [X_sol(2:end,:);X_sol(end,:)]; 316 | U0 = [U_sol(2:end,:);U_sol(end,:)]; 317 | end; 318 | toc 319 | 320 | figure(1) 321 | subplot(311) 322 | plot(t(N_MHE+1:end),X_estimate(:,1),'g','linewidth',1.5); hold on 323 | legend('Ground Truth','Measurement','MHE') 324 | 325 | subplot(312) 326 | plot(t(N_MHE+1:end),X_estimate(:,2),'g','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 327 | 328 | subplot(313) 329 | plot(t(N_MHE+1:end),X_estimate(:,3),'g','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 330 | 331 | 332 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MHE_code/shift.m: -------------------------------------------------------------------------------- 1 | function [t0, x0, u0] = shift(T, t0, x0, u,f) 2 | % add noise to the control actions before applying it 3 | con_cov = diag([0.005 deg2rad(2)]).^2; 4 | con = u(1,:)' + sqrt(con_cov)*randn(2,1); 5 | st = x0; 6 | 7 | f_value = f(st,con); 8 | st = st+ (T*f_value); 9 | 10 | x0 = full(st); 11 | t0 = t0 + T; 12 | 13 | u0 = [u(2:size(u,1),:);u(size(u,1),:)]; % shift the control action 14 | end -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Draw_MPC_PS_Obstacles.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_PS_Obstacles (t,xx,xx1,u_cl,xs,N,rob_diam,obs_x,obs_y,obs_diam) 2 | 3 | set(0,'DefaultAxesFontName', 'Times New Roman') 4 | set(0,'DefaultAxesFontSize', 12) 5 | 6 | line_width = 1.5; 7 | fontsize_labels = 14; 8 | 9 | %-------------------------------------------------------------------------- 10 | %-----------------------Simulate robots ----------------------------------- 11 | %-------------------------------------------------------------------------- 12 | x_r_1 = []; 13 | y_r_1 = []; 14 | 15 | 16 | 17 | r = rob_diam/2; % obstacle radius 18 | ang=0:0.005:2*pi; 19 | xp=r*cos(ang); 20 | yp=r*sin(ang); 21 | 22 | r = obs_diam/2; % obstacle radius 23 | xp_obs=r*cos(ang); 24 | yp_obs=r*sin(ang); 25 | 26 | figure(500) 27 | % Animate the robot motion 28 | %figure;%('Position',[200 200 1280 720]); 29 | set(gcf,'PaperPositionMode','auto') 30 | set(gcf, 'Color', 'w'); 31 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 32 | 33 | tic 34 | for k = 1:size(xx,2) 35 | h_t = 0.14; w_t=0.09; % triangle parameters 36 | 37 | x1 = xs(1); y1 = xs(2); th1 = xs(3); 38 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 39 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 40 | fill(x1_tri, y1_tri, 'g'); % plot reference state 41 | hold on; 42 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 43 | x_r_1 = [x_r_1 x1]; 44 | y_r_1 = [y_r_1 y1]; 45 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 46 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 47 | 48 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 49 | if k < size(xx,2) % plot prediction 50 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 51 | for j = 2:N+1 52 | plot(xx1(j,1,k)+xp,xx1(j,2,k)+yp,'--r'); % plot robot circle 53 | end 54 | end 55 | 56 | fill(x1_tri, y1_tri, 'r'); % plot robot position 57 | 58 | plot(x1+xp,y1+yp,'--r'); % plot robot circle 59 | 60 | plot(obs_x+xp_obs,obs_y+yp_obs,'--b'); % plot robot circle 61 | 62 | hold off 63 | %figure(500) 64 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 65 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 66 | axis([-0.4 1.8 -0.2 1.8]) 67 | pause(0.1) 68 | box on; 69 | grid on 70 | %aviobj = addframe(aviobj,gcf); 71 | drawnow 72 | % for video generation 73 | F(k) = getframe(gcf); % to get the current frame 74 | end 75 | toc 76 | close(gcf) 77 | %viobj = close(aviobj) 78 | video = VideoWriter('exp.avi','Uncompressed AVI'); 79 | 80 | video = VideoWriter('exp.avi','Motion JPEG AVI'); 81 | video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 82 | open(video) 83 | writeVideo(video,F) 84 | close (video) 85 | 86 | figure 87 | subplot(211) 88 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.75 0.75]) 89 | ylabel('v (rad/s)') 90 | grid on 91 | subplot(212) 92 | stairs(t,u_cl(:,2),'r','linewidth',1.5); axis([0 t(end) -0.85 0.85]) 93 | xlabel('time (seconds)') 94 | ylabel('\omega (rad/s)') 95 | grid on 96 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Draw_MPC_point_stabilization_v1.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 2 | 3 | 4 | set(0,'DefaultAxesFontName', 'Times New Roman') 5 | set(0,'DefaultAxesFontSize', 12) 6 | 7 | line_width = 1.5; 8 | fontsize_labels = 14; 9 | 10 | %-------------------------------------------------------------------------- 11 | %-----------------------Simulate robots ----------------------------------- 12 | %-------------------------------------------------------------------------- 13 | x_r_1 = []; 14 | y_r_1 = []; 15 | 16 | 17 | 18 | r = rob_diam/2; % obstacle radius 19 | ang=0:0.005:2*pi; 20 | xp=r*cos(ang); 21 | yp=r*sin(ang); 22 | 23 | figure(500) 24 | % Animate the robot motion 25 | %figure;%('Position',[200 200 1280 720]); 26 | set(gcf,'PaperPositionMode','auto') 27 | set(gcf, 'Color', 'w'); 28 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 29 | 30 | for k = 1:size(xx,2) 31 | h_t = 0.14; w_t=0.09; % triangle parameters 32 | 33 | x1 = xs(1); y1 = xs(2); th1 = xs(3); 34 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 35 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 36 | fill(x1_tri, y1_tri, 'g'); % plot reference state 37 | hold on; 38 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 39 | x_r_1 = [x_r_1 x1]; 40 | y_r_1 = [y_r_1 y1]; 41 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 42 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 43 | 44 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 45 | if k < size(xx,2) % plot prediction 46 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 47 | end 48 | 49 | fill(x1_tri, y1_tri, 'r'); % plot robot position 50 | plot(x1+xp,y1+yp,'--r'); % plot robot circle 51 | 52 | 53 | hold off 54 | %figure(500) 55 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 56 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 57 | axis([-0.2 1.8 -0.2 1.8]) 58 | pause(0.1) 59 | box on; 60 | grid on 61 | %aviobj = addframe(aviobj,gcf); 62 | drawnow 63 | % for video generation 64 | F(k) = getframe(gcf); % to get the current frame 65 | end 66 | close(gcf) 67 | %viobj = close(aviobj) 68 | %video = VideoWriter('exp.avi','Uncompressed AVI'); 69 | 70 | % video = VideoWriter('exp.avi','Motion JPEG AVI'); 71 | % video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 72 | % open(video) 73 | % writeVideo(video,F) 74 | % close (video) 75 | 76 | figure 77 | subplot(211) 78 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.35 0.75]) 79 | ylabel('v (rad/s)') 80 | grid on 81 | subplot(212) 82 | stairs(t,u_cl(:,2),'r','linewidth',1.5); axis([0 t(end) -0.85 0.85]) 83 | xlabel('time (seconds)') 84 | ylabel('\omega (rad/s)') 85 | grid on 86 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Draw_MPC_tracking_v1.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 2 | 3 | 4 | set(0,'DefaultAxesFontName', 'Times New Roman') 5 | set(0,'DefaultAxesFontSize', 12) 6 | 7 | line_width = 1.5; 8 | fontsize_labels = 14; 9 | 10 | %-------------------------------------------------------------------------- 11 | %-----------------------Simulate robots ----------------------------------- 12 | %-------------------------------------------------------------------------- 13 | x_r_1 = []; 14 | y_r_1 = []; 15 | 16 | r = rob_diam/2; % obstacle radius 17 | ang=0:0.005:2*pi; 18 | xp=r*cos(ang); 19 | yp=r*sin(ang); 20 | 21 | figure(500) 22 | % Animate the robot motion 23 | %figure;%('Position',[200 200 1280 720]); 24 | set(gcf,'PaperPositionMode','auto') 25 | set(gcf, 'Color', 'w'); 26 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 27 | 28 | for k = 1:size(xx,2) 29 | plot([0 12],[1 1],'-g','linewidth',1.2);hold on % plot the reference trajectory 30 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 31 | x_r_1 = [x_r_1 x1]; 32 | y_r_1 = [y_r_1 y1]; 33 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 34 | if k < size(xx,2) % plot prediction 35 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 36 | end 37 | 38 | plot(x1,y1,'-sk','MarkerSize',25)% plot robot position 39 | hold off 40 | %figure(500) 41 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 42 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 43 | axis([-1 16 -0.5 1.5]) 44 | pause(0.2) 45 | box on; 46 | grid on 47 | %aviobj = addframe(aviobj,gcf); 48 | drawnow 49 | % for video generation 50 | F(k) = getframe(gcf); % to get the current frame 51 | end 52 | close(gcf) 53 | %viobj = close(aviobj) 54 | %video = VideoWriter('exp.avi','Uncompressed AVI'); 55 | 56 | % video = VideoWriter('exp.avi','Motion JPEG AVI'); 57 | % video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 58 | % open(video) 59 | % writeVideo(video,F) 60 | % close (video) 61 | 62 | figure 63 | subplot(211) 64 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.2 0.8]) 65 | ylabel('v (rad/s)') 66 | grid on 67 | subplot(212) 68 | stairs(t,u_cl(:,2),'r','linewidth',1.5); %axis([0 t(end) -0.85 0.85]) 69 | xlabel('time (seconds)') 70 | ylabel('\omega (rad/s)') 71 | grid on 72 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Sim_1_MPC_Robot_PS_sing_shooting.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Single shooting 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; % sampling time [s] 11 | N = 100; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include the initial and the reference state of the robot) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A Matrix that represents the states over the optimization problem. 31 | 32 | % compute solution symbolically 33 | X(:,1) = P(1:3); % initial state 34 | for k = 1:N 35 | st = X(:,k); con = U(:,k); 36 | f_value = f(st,con); 37 | st_next = st+ (T*f_value); 38 | X(:,k+1) = st_next; 39 | end 40 | % this function to get the optimal trajectory knowing the optimal solution 41 | ff=Function('ff',{U,P},{X}); 42 | 43 | obj = 0; % Objective function 44 | g = []; % constraints vector 45 | 46 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 47 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 48 | % compute objective 49 | for k=1:N 50 | st = X(:,k); con = U(:,k); 51 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 52 | end 53 | 54 | % compute constraints 55 | for k = 1:N+1 % box constraints due to the map margins 56 | g = [g ; X(1,k)]; %state x 57 | g = [g ; X(2,k)]; %state y 58 | end 59 | 60 | % make the decision variables one column vector 61 | OPT_variables = reshape(U,2*N,1); 62 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 63 | 64 | opts = struct; 65 | opts.ipopt.max_iter = 100; 66 | opts.ipopt.print_level =0;%0,3 67 | opts.print_time = 0; 68 | opts.ipopt.acceptable_tol =1e-8; 69 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 70 | 71 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 72 | 73 | 74 | args = struct; 75 | % inequality constraints (state constraints) 76 | args.lbg = -2; % lower bound of the states x and y 77 | args.ubg = 2; % upper bound of the states x and y 78 | 79 | % input constraints 80 | args.lbx(1:2:2*N-1,1) = v_min; args.lbx(2:2:2*N,1) = omega_min; 81 | args.ubx(1:2:2*N-1,1) = v_max; args.ubx(2:2:2*N,1) = omega_max; 82 | 83 | 84 | %---------------------------------------------- 85 | % ALL OF THE ABOVE IS JUST A PROBLEM SETTING UP 86 | 87 | 88 | % THE SIMULATION LOOP SHOULD START FROM HERE 89 | %------------------------------------------- 90 | t0 = 0; 91 | x0 = [0 ; 0 ; 0.0]; % initial condition. 92 | xs = [1.5 ; 1.5 ; 0]; % Reference posture. 93 | 94 | xx(:,1) = x0; % xx contains the history of states 95 | t(1) = t0; 96 | 97 | u0 = zeros(N,2); % two control inputs 98 | 99 | sim_tim = 20; % Maximum simulation time 100 | 101 | % Start MPC 102 | mpciter = 0; 103 | xx1 = []; 104 | u_cl=[]; 105 | 106 | 107 | % the main simulaton loop... it works as long as the error is greater 108 | % than 10^-2 and the number of mpc steps is less than its maximum 109 | % value. 110 | main_loop = tic; 111 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 112 | args.p = [x0;xs]; % set the values of the parameters vector 113 | args.x0 = reshape(u0',2*N,1); % initial value of the optimization variables 114 | %tic 115 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 116 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 117 | %toc 118 | u = reshape(full(sol.x)',2,N)'; 119 | ff_value = ff(u',args.p); % compute OPTIMAL solution TRAJECTORY 120 | xx1(:,1:3,mpciter+1)= full(ff_value)'; 121 | 122 | u_cl= [u_cl ; u(1,:)]; 123 | t(mpciter+1) = t0; 124 | [t0, x0, u0] = shift(T, t0, x0, u,f); % get the initialization of the next optimization step 125 | 126 | xx(:,mpciter+2) = x0; 127 | mpciter 128 | mpciter = mpciter + 1; 129 | end; 130 | main_loop_time = toc(main_loop); 131 | ss_error = norm((x0-xs),2) 132 | average_mpc_time = main_loop_time/(mpciter+1) 133 | 134 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) % a drawing function 135 | 136 | 137 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Sim_2_MPC_R_PS_mul_shooting_RK.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Multiple shooting + Runge Kutta 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.5.1 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.1') 8 | import casadi.* 9 | 10 | h = 0.2; %[s] 11 | N = 10; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include the initial state and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | k1 = f(st, con); % new 45 | k2 = f(st + h/2*k1, con); % new 46 | k3 = f(st + h/2*k2, con); % new 47 | k4 = f(st + h*k3, con); % new 48 | st_next_RK4=st +h/6*(k1 +2*k2 +2*k3 +k4); % new 49 | % f_value = f(st,con); 50 | % st_next_euler = st+ (h*f_value); 51 | % g = [g;st_next-st_next_euler]; % compute constraints 52 | g = [g;st_next-st_next_RK4]; % compute constraints % new 53 | end 54 | % make the decision variable one column vector 55 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 56 | 57 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 58 | 59 | opts = struct; 60 | opts.ipopt.max_iter = 2000; 61 | opts.ipopt.print_level =0;%0,3 62 | opts.print_time = 0; 63 | opts.ipopt.acceptable_tol =1e-8; 64 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 65 | 66 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 67 | 68 | args = struct; 69 | 70 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 71 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 72 | 73 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 74 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 75 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 76 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 77 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 78 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 79 | 80 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 81 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 82 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 83 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 84 | %---------------------------------------------- 85 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 86 | 87 | 88 | % THE SIMULATION LOOP SHOULD START FROM HERE 89 | %------------------------------------------- 90 | t0 = 0; 91 | x0 = [0 ; 0 ; 0.0]; % initial condition. 92 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 93 | 94 | xx(:,1) = x0; % xx contains the history of states 95 | t(1) = t0; 96 | 97 | u0 = zeros(N,2); % two control inputs for each robot 98 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 99 | 100 | sim_tim = 20; % Maximum simulation time 101 | 102 | % Start MPC 103 | mpciter = 0; 104 | xx1 = []; 105 | u_cl=[]; 106 | 107 | % the main simulaton loop... it works as long as the error is greater 108 | % than 10^-6 and the number of mpc steps is less than its maximum 109 | % value. 110 | main_loop = tic; 111 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / h) 112 | args.p = [x0;xs]; % set the values of the parameters vector 113 | % initial value of the optimization variables 114 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 115 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 116 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 117 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 118 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 119 | u_cl= [u_cl ; u(1,:)]; 120 | t(mpciter+1) = t0; 121 | % Apply the control and shift the solution 122 | [t0, x0, u0] = shift(h, t0, x0, u,f); 123 | xx(:,mpciter+2) = x0; 124 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 125 | % Shift trajectory to initialize the next step 126 | X0 = [X0(2:end,:);X0(end,:)]; 127 | mpciter 128 | mpciter = mpciter + 1; 129 | end; 130 | main_loop_time = toc(main_loop); 131 | ss_error = norm((x0-xs),2) 132 | average_mpc_time = main_loop_time/(mpciter+1) 133 | 134 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 135 | 136 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Sim_2_MPC_Robot_PS_mul_shooting.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Multiple shooting 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 100; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include the initial state and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % make the decision variable one column vector 49 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 50 | 51 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 52 | 53 | opts = struct; 54 | opts.ipopt.max_iter = 2000; 55 | opts.ipopt.print_level =0;%0,3 56 | opts.print_time = 0; 57 | opts.ipopt.acceptable_tol =1e-8; 58 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 59 | 60 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 61 | 62 | args = struct; 63 | 64 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 65 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 66 | 67 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 68 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 69 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 70 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 71 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 72 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 73 | 74 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 75 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 76 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 77 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 78 | %---------------------------------------------- 79 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 80 | 81 | 82 | % THE SIMULATION LOOP SHOULD START FROM HERE 83 | %------------------------------------------- 84 | t0 = 0; 85 | x0 = [0 ; 0 ; 0.0]; % initial condition. 86 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 87 | 88 | xx(:,1) = x0; % xx contains the history of states 89 | t(1) = t0; 90 | 91 | u0 = zeros(N,2); % two control inputs for each robot 92 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 93 | 94 | sim_tim = 20; % Maximum simulation time 95 | 96 | % Start MPC 97 | mpciter = 0; 98 | xx1 = []; 99 | u_cl=[]; 100 | 101 | % the main simulaton loop... it works as long as the error is greater 102 | % than 10^-6 and the number of mpc steps is less than its maximum 103 | % value. 104 | main_loop = tic; 105 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 106 | args.p = [x0;xs]; % set the values of the parameters vector 107 | % initial value of the optimization variables 108 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 109 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 110 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 111 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 112 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 113 | u_cl= [u_cl ; u(1,:)]; 114 | t(mpciter+1) = t0; 115 | % Apply the control and shift the solution 116 | [t0, x0, u0] = shift(T, t0, x0, u,f); 117 | xx(:,mpciter+2) = x0; 118 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 119 | % Shift trajectory to initialize the next step 120 | X0 = [X0(2:end,:);X0(end,:)]; 121 | mpciter 122 | mpciter = mpciter + 1; 123 | end; 124 | main_loop_time = toc(main_loop); 125 | ss_error = norm((x0-xs),2) 126 | average_mpc_time = main_loop_time/(mpciter+1) 127 | 128 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 129 | 130 | 131 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Sim_3_MPC_Robot_PS_obs_avoid_mul_sh.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Multiple shooting + obstacle avoidance 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.2; %[s] 11 | N = 10; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | P = SX.sym('P',n_states + n_states); 27 | % parameters (which include at the initial state of the robot and the reference state) 28 | 29 | X = SX.sym('X',n_states,(N+1)); 30 | % A vector that represents the states over the optimization problem. 31 | 32 | obj = 0; % Objective function 33 | g = []; % constraints vector 34 | 35 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 36 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 37 | 38 | st = X(:,1); % initial state 39 | g = [g;st-P(1:3)]; % initial condition constraints 40 | for k = 1:N 41 | st = X(:,k); con = U(:,k); 42 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 43 | st_next = X(:,k+1); 44 | f_value = f(st,con); 45 | st_next_euler = st+ (T*f_value); 46 | g = [g;st_next-st_next_euler]; % compute constraints 47 | end 48 | % Add constraints for collision avoidance 49 | obs_x = 0.5; % meters 50 | obs_y = 0.5; % meters 51 | obs_diam = 0.3; % meters 52 | for k = 1:N+1 % box constraints due to the map margins 53 | g = [g ; -sqrt((X(1,k)-obs_x)^2+(X(2,k)-obs_y)^2) + (rob_diam/2 + obs_diam/2)]; 54 | end 55 | % make the decision variable one column vector 56 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 57 | 58 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 59 | 60 | opts = struct; 61 | opts.ipopt.max_iter = 100; 62 | opts.ipopt.print_level =0;%0,3 63 | opts.print_time = 0; 64 | opts.ipopt.acceptable_tol =1e-8; 65 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 66 | 67 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 68 | 69 | args = struct; 70 | args.lbg(1:3*(N+1)) = 0; % equality constraints 71 | args.ubg(1:3*(N+1)) = 0; % equality constraints 72 | 73 | args.lbg(3*(N+1)+1 : 3*(N+1)+ (N+1)) = -inf; % inequality constraints 74 | args.ubg(3*(N+1)+1 : 3*(N+1)+ (N+1)) = 0; % inequality constraints 75 | 76 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 77 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 78 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 79 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 80 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 81 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 82 | 83 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 84 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 85 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 86 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 87 | %---------------------------------------------- 88 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 89 | 90 | 91 | % THE SIMULATION LOOP SHOULD START FROM HERE 92 | %------------------------------------------- 93 | t0 = 0; 94 | x0 = [0 ; 0 ; 0.0]; % initial condition. 95 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 96 | 97 | xx(:,1) = x0; % xx contains the history of states 98 | t(1) = t0; 99 | 100 | u0 = zeros(N,2); % two control inputs for each robot 101 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 102 | 103 | sim_tim = 20; % Maximum simulation time 104 | 105 | % Start MPC 106 | mpciter = 0; 107 | xx1 = []; 108 | u_cl=[]; 109 | 110 | % the main simulaton loop... it works as long as the error is greater 111 | % than 10^-6 and the number of mpc steps is less than its maximum 112 | % value. 113 | main_loop = tic; 114 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 115 | args.p = [x0;xs]; % set the values of the parameters vector 116 | % initial value of the optimization variables 117 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 118 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 119 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 120 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 121 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 122 | u_cl= [u_cl ; u(1,:)]; 123 | t(mpciter+1) = t0; 124 | % Apply the control and shift the solution 125 | [t0, x0, u0] = shift(T, t0, x0, u,f); 126 | xx(:,mpciter+2) = x0; 127 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 128 | % Shift trajectory to initialize the next step 129 | X0 = [X0(2:end,:);X0(end,:)]; 130 | mpciter 131 | mpciter = mpciter + 1; 132 | end; 133 | main_loop_time = toc(main_loop); 134 | ss_error = norm((x0-xs),2) 135 | average_mpc_time = main_loop_time/(mpciter+1) 136 | 137 | Draw_MPC_PS_Obstacles (t,xx,xx1,u_cl,xs,N,rob_diam,obs_x,obs_y,obs_diam) 138 | 139 | 140 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/Sim_4_MPC_Robot_tracking_mul_shooting.m: -------------------------------------------------------------------------------- 1 | % Trajectory Tracking + Multiple shooting 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | import casadi.* 9 | 10 | T = 0.5; %[s] 11 | N = 8; % prediction horizon 12 | rob_diam = 0.3; 13 | 14 | v_max = 0.6; v_min = -v_max; 15 | omega_max = pi/4; omega_min = -omega_max; 16 | 17 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 18 | states = [x;y;theta]; n_states = length(states); 19 | 20 | v = SX.sym('v'); omega = SX.sym('omega'); 21 | controls = [v;omega]; n_controls = length(controls); 22 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 23 | 24 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 25 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 26 | %P = SX.sym('P',n_states + n_states); 27 | P = SX.sym('P',n_states + N*(n_states+n_controls)); 28 | % parameters (which include the initial state and the reference along the 29 | % predicted trajectory (reference states and reference controls)) 30 | 31 | X = SX.sym('X',n_states,(N+1)); 32 | % A vector that represents the states over the optimization problem. 33 | 34 | obj = 0; % Objective function 35 | g = []; % constraints vector 36 | 37 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 1;Q(3,3) = 0.5; % weighing matrices (states) 38 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 39 | 40 | st = X(:,1); % initial state 41 | g = [g;st-P(1:3)]; % initial condition constraints 42 | for k = 1:N 43 | st = X(:,k); con = U(:,k); 44 | %obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 45 | obj = obj+(st-P(5*k-1:5*k+1))'*Q*(st-P(5*k-1:5*k+1)) + ... 46 | (con-P(5*k+2:5*k+3))'*R*(con-P(5*k+2:5*k+3)) ; % calculate obj 47 | % the number 5 is (n_states+n_controls) 48 | st_next = X(:,k+1); 49 | f_value = f(st,con); 50 | st_next_euler = st+ (T*f_value); 51 | g = [g;st_next-st_next_euler]; % compute constraints 52 | end 53 | % make the decision variable one column vector 54 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 55 | 56 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 57 | 58 | opts = struct; 59 | opts.ipopt.max_iter = 2000; 60 | opts.ipopt.print_level =0;%0,3 61 | opts.print_time = 0; 62 | opts.ipopt.acceptable_tol =1e-8; 63 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 64 | 65 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 66 | 67 | args = struct; 68 | 69 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 70 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 71 | 72 | args.lbx(1:3:3*(N+1),1) = -20; %state x lower bound % new - adapt the bound 73 | args.ubx(1:3:3*(N+1),1) = 20; %state x upper bound % new - adapt the bound 74 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 75 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 76 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 77 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 78 | 79 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 80 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 81 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 82 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 83 | %---------------------------------------------- 84 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 85 | 86 | 87 | % THE SIMULATION LOOP SHOULD START FROM HERE 88 | %------------------------------------------- 89 | t0 = 0; 90 | x0 = [0 ; 0 ; 0.0]; % initial condition. 91 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 92 | 93 | xx(:,1) = x0; % xx contains the history of states 94 | t(1) = t0; 95 | 96 | u0 = zeros(N,2); % two control inputs for each robot 97 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 98 | 99 | sim_tim = 30; % Maximum simulation time 100 | 101 | % Start MPC 102 | mpciter = 0; 103 | xx1 = []; 104 | u_cl=[]; 105 | 106 | % the main simulaton loop... it works as long as the error is greater 107 | % than 10^-6 and the number of mpc steps is less than its maximum 108 | % value. 109 | main_loop = tic; 110 | while(mpciter < sim_tim / T) % new - condition for ending the loop 111 | current_time = mpciter*T; %new - get the current time 112 | % args.p = [x0;xs]; % set the values of the parameters vector 113 | %---------------------------------------------------------------------- 114 | args.p(1:3) = x0; % initial condition of the robot posture 115 | for k = 1:N %new - set the reference to track 116 | t_predict = current_time + (k-1)*T; % predicted time instant 117 | x_ref = 0.5*t_predict; y_ref = 1; theta_ref = 0; 118 | u_ref = 0.5; omega_ref = 0; 119 | if x_ref >= 12 % the trajectory end is reached 120 | x_ref = 12; y_ref = 1; theta_ref = 0; 121 | u_ref = 0; omega_ref = 0; 122 | end 123 | args.p(5*k-1:5*k+1) = [x_ref, y_ref, theta_ref]; 124 | args.p(5*k+2:5*k+3) = [u_ref, omega_ref]; 125 | end 126 | %---------------------------------------------------------------------- 127 | % initial value of the optimization variables 128 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 129 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 130 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 131 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 132 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 133 | u_cl= [u_cl ; u(1,:)]; 134 | t(mpciter+1) = t0; 135 | % Apply the control and shift the solution 136 | [t0, x0, u0] = shift(T, t0, x0, u,f); 137 | xx(:,mpciter+2) = x0; 138 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 139 | % Shift trajectory to initialize the next step 140 | X0 = [X0(2:end,:);X0(end,:)]; 141 | mpciter 142 | mpciter = mpciter + 1; 143 | end; 144 | main_loop_time = toc(main_loop); 145 | average_mpc_time = main_loop_time/(mpciter+1) 146 | 147 | Draw_MPC_tracking_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 148 | 149 | 150 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/MPC_code/shift.m: -------------------------------------------------------------------------------- 1 | function [t0, x0, u0] = shift(T, t0, x0, u,f) 2 | st = x0; 3 | con = u(1,:)'; 4 | f_value = f(st,con); 5 | st = st+ (T*f_value); 6 | x0 = full(st); 7 | 8 | t0 = t0 + T; 9 | u0 = [u(2:size(u,1),:);u(size(u,1),:)]; 10 | end -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/motivation_example_1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_4_5/motivation_example_1.m -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/motivation_example_2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_4_5/motivation_example_2.m -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/motivation_example_3.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_4_5/motivation_example_3.m -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_4_5/motivation_example_3.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_4_5/motivation_example_3.xlsx -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MHE_code/Draw_MPC_point_stabilization_v1.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 2 | 3 | 4 | set(0,'DefaultAxesFontName', 'Times New Roman') 5 | set(0,'DefaultAxesFontSize', 12) 6 | 7 | line_width = 1.5; 8 | fontsize_labels = 14; 9 | 10 | %-------------------------------------------------------------------------- 11 | %-----------------------Simulate robots ----------------------------------- 12 | %-------------------------------------------------------------------------- 13 | x_r_1 = []; 14 | y_r_1 = []; 15 | 16 | 17 | 18 | r = rob_diam/2; % obstacle radius 19 | ang=0:0.005:2*pi; 20 | xp=r*cos(ang); 21 | yp=r*sin(ang); 22 | 23 | figure(500) 24 | % Animate the robot motion 25 | %figure;%('Position',[200 200 1280 720]); 26 | set(gcf,'PaperPositionMode','auto') 27 | set(gcf, 'Color', 'w'); 28 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 29 | 30 | tic 31 | for k = 1:size(xx,2) 32 | h_t = 0.14; w_t=0.09; % triangle parameters 33 | 34 | x1 = xs(1); y1 = xs(2); th1 = xs(3); 35 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 36 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 37 | fill(x1_tri, y1_tri, 'g'); % plot reference state 38 | hold on; 39 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 40 | x_r_1 = [x_r_1 x1]; 41 | y_r_1 = [y_r_1 y1]; 42 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 43 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 44 | 45 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 46 | if k < size(xx,2) % plot prediction 47 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 48 | end 49 | 50 | fill(x1_tri, y1_tri, 'r'); % plot robot position 51 | plot(x1+xp,y1+yp,'--r'); % plot robot circle 52 | 53 | 54 | hold off 55 | %figure(500) 56 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 57 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 58 | axis([-0.2 1.8 -0.2 1.8]) 59 | pause(0.0) 60 | box on; 61 | grid on 62 | %aviobj = addframe(aviobj,gcf); 63 | drawnow 64 | % for video generation 65 | F(k) = getframe(gcf); % to get the current frame 66 | end 67 | toc 68 | close(gcf) 69 | %viobj = close(aviobj) 70 | %video = VideoWriter('exp.avi','Uncompressed AVI'); 71 | 72 | % video = VideoWriter('exp.avi','Motion JPEG AVI'); 73 | % video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 74 | % open(video) 75 | % writeVideo(video,F) 76 | % close (video) 77 | 78 | figure 79 | subplot(211) 80 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.35 0.75]) 81 | ylabel('v (rad/s)') 82 | grid on 83 | subplot(212) 84 | stairs(t,u_cl(:,2),'r','linewidth',1.5); axis([0 t(end) -0.85 0.85]) 85 | xlabel('time (seconds)') 86 | ylabel('\omega (rad/s)') 87 | grid on 88 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MHE_code/MHE_Robot_PS_mul_shooting_v1.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.1.1 7 | % addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | % CasADi v3.5.5 9 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5') 10 | import casadi.* 11 | 12 | T = 0.2; %[s] 13 | N = 10; % prediction horizon 14 | rob_diam = 0.3; 15 | 16 | v_max = 0.6; v_min = -v_max; 17 | omega_max = pi/4; omega_min = -omega_max; 18 | 19 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 20 | states = [x;y;theta]; n_states = length(states); 21 | 22 | v = SX.sym('v'); omega = SX.sym('omega'); 23 | controls = [v;omega]; n_controls = length(controls); 24 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 25 | 26 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 27 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 28 | P = SX.sym('P',n_states + n_states); 29 | % parameters (which include at the initial state of the robot and the reference state) 30 | 31 | X = SX.sym('X',n_states,(N+1)); 32 | % A vector that represents the states over the optimization problem. 33 | 34 | obj = 0; % Objective function 35 | g = []; % constraints vector 36 | 37 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 38 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 39 | 40 | st = X(:,1); % initial state 41 | g = [g;st-P(1:3)]; % initial condition constraints 42 | for k = 1:N 43 | st = X(:,k); con = U(:,k); 44 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 45 | st_next = X(:,k+1); 46 | f_value = f(st,con); 47 | st_next_euler = st+ (T*f_value); 48 | g = [g;st_next-st_next_euler]; % compute constraints 49 | end 50 | % make the decision variable one column vector 51 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 52 | 53 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 54 | 55 | opts = struct; 56 | opts.ipopt.max_iter = 2000; 57 | opts.ipopt.print_level =0;%0,3 58 | opts.print_time = 0; 59 | opts.ipopt.acceptable_tol =1e-8; 60 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 61 | 62 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 63 | 64 | 65 | args = struct; 66 | 67 | args.lbg(1:3*(N+1)) = 0; 68 | args.ubg(1:3*(N+1)) = 0; 69 | 70 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 71 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 72 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 73 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 74 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 75 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 76 | 77 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 78 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 79 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 80 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 81 | %---------------------------------------------- 82 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 83 | 84 | 85 | % THE SIMULATION LOOP SHOULD START FROM HERE 86 | %------------------------------------------- 87 | t0 = 0; 88 | x0 = [0.1 ; 0.1 ; 0.0]; % initial condition. 89 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 90 | 91 | xx(:,1) = x0; % xx contains the history of states 92 | t(1) = t0; 93 | 94 | u0 = zeros(N,2); % two control inputs for each robot 95 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 96 | 97 | sim_tim = 20; % total sampling times 98 | 99 | % Start MPC 100 | mpciter = 0; 101 | xx1 = []; 102 | u_cl=[]; 103 | 104 | % the main simulaton loop... it works as long as the error is greater 105 | % than 10^-6 and the number of mpc steps is less than its maximum 106 | % value. 107 | tic 108 | while(norm((x0-xs),2) > 0.05 && mpciter < sim_tim / T) 109 | args.p = [x0;xs]; % set the values of the parameters vector 110 | % initial value of the optimization variables 111 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 112 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 113 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 114 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 115 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 116 | u_cl= [u_cl ; u(1,:)]; 117 | t(mpciter+1) = t0; 118 | % Apply the control and shift the solution 119 | [t0, x0, u0] = shift(T, t0, x0, u,f); 120 | xx(:,mpciter+2) = x0; 121 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 122 | % Shift trajectory to initialize the next step 123 | X0 = [X0(2:end,:);X0(end,:)]; 124 | mpciter 125 | mpciter = mpciter + 1; 126 | end; 127 | toc 128 | 129 | ss_error = norm((x0-xs),2) 130 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 131 | %----------------------------------------- 132 | %----------------------------------------- 133 | %----------------------------------------- 134 | % Start MHE implementation from here 135 | %----------------------------------------- 136 | %----------------------------------------- 137 | %----------------------------------------- 138 | % plot the ground truth 139 | figure(1) 140 | subplot(311) 141 | plot(t,xx(1,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 142 | ylabel('x (m)') 143 | grid on 144 | subplot(312) 145 | plot(t,xx(2,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 146 | ylabel('y (m)') 147 | grid on 148 | subplot(313) 149 | plot(t,xx(3,1:end-1),'b','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 150 | xlabel('time (seconds)') 151 | ylabel('\theta (rad)') 152 | grid on 153 | 154 | % Synthesize the measurments 155 | con_cov = diag([0.005 deg2rad(2)]).^2; 156 | meas_cov = diag([0.1 deg2rad(2)]).^2; 157 | 158 | r = []; 159 | alpha = []; 160 | for k = 1: length(xx(1,:))-1 161 | r = [r; sqrt(xx(1,k)^2+xx(2,k)^2) + sqrt(meas_cov(1,1))*randn(1)]; 162 | alpha = [alpha; atan(xx(2,k)/xx(1,k)) + sqrt(meas_cov(2,2))*randn(1)]; 163 | end 164 | y_measurements = [ r , alpha ]; 165 | 166 | % Plot the cartesian coordinates from the measurements used 167 | figure(1) 168 | subplot(311) 169 | plot(t,r.*cos(alpha),'r','linewidth',1.5); hold on 170 | grid on 171 | legend('Ground Truth','Measurement') 172 | subplot(312) 173 | plot(t,r.*sin(alpha),'r','linewidth',1.5); hold on 174 | grid on 175 | 176 | % plot the ground truth mesurements VS the noisy measurements 177 | figure(2) 178 | subplot(211) 179 | plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on 180 | plot(t,r,'r','linewidth',1.5); axis([0 t(end) -0.2 3]); hold on 181 | ylabel('Range: [ r (m) ]') 182 | grid on 183 | legend('Ground Truth','Measurement') 184 | subplot(212) 185 | plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on 186 | plot(t,alpha,'r','linewidth',1.5); axis([0 t(end) 0.2 1]); hold on 187 | ylabel('Bearing: [ \alpha (rad) ]') 188 | grid on 189 | 190 | % The following two matrices contain what we know about the system, i.e. 191 | % the nominal control actions applied (measured) and the range and bearing 192 | % measurements. 193 | %------------------------------------------------------------------------- 194 | u_cl; 195 | y_measurements; 196 | 197 | % Let's now formulate our MHE problem 198 | %------------------------------------ 199 | T = 0.2; %[s] 200 | N_MHE = size(y_measurements,1)-1; % Estimation horizon 201 | 202 | v_max = 0.6; v_min = -v_max; 203 | omega_max = pi/4; omega_min = -omega_max; 204 | 205 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 206 | states = [x;y;theta]; n_states = length(states); 207 | v = SX.sym('v'); omega = SX.sym('omega'); 208 | controls = [v;omega]; n_controls = length(controls); 209 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 210 | f = Function('f',{states,controls},{rhs}); % MOTION MODEL 211 | 212 | r = SX.sym('r'); alpha = SX.sym('alpha'); % range and bearing 213 | measurement_rhs = [sqrt(x^2+y^2); atan(y/x)]; 214 | h = Function('h',{states},{measurement_rhs}); % MEASUREMENT MODEL 215 | %y_tilde = [r;alpha]; 216 | 217 | % Decision variables 218 | U = SX.sym('U',n_controls,N_MHE); %(controls) 219 | X = SX.sym('X',n_states,(N_MHE+1)); %(states) [remember multiple shooting] 220 | 221 | P = SX.sym('P', 2 , N_MHE + (N_MHE+1)); 222 | % parameters (include r and alpha measurements as well as controls measurements) 223 | 224 | V = inv(sqrt(meas_cov)); % weighing matrices (output) y_tilde - y 225 | W = inv(sqrt(con_cov)); % weighing matrices (input) u_tilde - u 226 | 227 | obj = 0; % Objective function 228 | g = []; % constraints vector 229 | for k = 1:N_MHE+1 230 | st = X(:,k); 231 | h_x = h(st); 232 | y_tilde = P(:,k); 233 | obj = obj+ (y_tilde-h_x)' * V * (y_tilde-h_x); % calculate obj 234 | end 235 | 236 | for k = 1:N_MHE 237 | con = U(:,k); 238 | u_tilde = P(:,N_MHE+ k); 239 | obj = obj+ (u_tilde-con)' * W * (u_tilde-con); % calculate obj 240 | end 241 | 242 | % multiple shooting constraints 243 | for k = 1:N_MHE 244 | st = X(:,k); con = U(:,k); 245 | st_next = X(:,k+1); 246 | f_value = f(st,con); 247 | st_next_euler = st+ (T*f_value); 248 | g = [g;st_next-st_next_euler]; % compute constraints 249 | end 250 | 251 | 252 | % make the decision variable one column vector 253 | OPT_variables = [reshape(X,3*(N_MHE+1),1);reshape(U,2*N_MHE,1)]; 254 | 255 | nlp_mhe = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 256 | 257 | opts = struct; 258 | opts.ipopt.max_iter = 2000; 259 | opts.ipopt.print_level =0;%0,3 260 | opts.print_time = 0; 261 | opts.ipopt.acceptable_tol =1e-8; 262 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 263 | 264 | solver = nlpsol('solver', 'ipopt', nlp_mhe,opts); 265 | 266 | args = struct; 267 | 268 | args.lbg(1:3*(N_MHE)) = 0; % equality constraints 269 | args.ubg(1:3*(N_MHE)) = 0; % equality constraints 270 | 271 | args.lbx(1:3:3*(N_MHE+1),1) = -2; %state x lower bound 272 | args.ubx(1:3:3*(N_MHE+1),1) = 2; %state x upper bound 273 | args.lbx(2:3:3*(N_MHE+1),1) = -2; %state y lower bound 274 | args.ubx(2:3:3*(N_MHE+1),1) = 2; %state y upper bound 275 | args.lbx(3:3:3*(N_MHE+1),1) = -pi/2; %state theta lower bound 276 | args.ubx(3:3:3*(N_MHE+1),1) = pi/2; %state theta upper bound 277 | 278 | args.lbx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_min; %v lower bound 279 | args.ubx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_max; %v upper bound 280 | args.lbx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_min; %omega lower bound 281 | args.ubx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_max; %omega upper bound 282 | %---------------------------------------------- 283 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 284 | 285 | % MHE Simulation 286 | %------------------------------------------ 287 | U0 = zeros(N_MHE,2); % two control inputs for each robot 288 | X0 = zeros(N_MHE+1,3); % initialization of the states decision variables 289 | 290 | U0 = u_cl(1:N_MHE,:); % initialize the control actions by the measured 291 | % initialize the states from the measured range and bearing 292 | X0(:,1:2) = [y_measurements(1:N_MHE+1,1).*cos(y_measurements(1:N_MHE+1,2)),... 293 | y_measurements(1:N_MHE+1,1).*sin(y_measurements(1:N_MHE+1,2))]; 294 | 295 | k=1; 296 | % Get the measurements window and put it as parameters in p 297 | args.p = [y_measurements(k:k+N_MHE,:)',u_cl(k:k+N_MHE-1,:)']; 298 | % initial value of the optimization variables 299 | args.x0 = [reshape(X0',3*(N_MHE+1),1);reshape(U0',2*N_MHE,1)]; 300 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 301 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 302 | U_sol = reshape(full(sol.x(3*(N_MHE+1)+1:end))',2,N_MHE)'; % get controls only from the solution 303 | X_sol = reshape(full(sol.x(1:3*(N_MHE+1)))',3,N_MHE+1)'; % get solution TRAJECTORY 304 | 305 | figure(1) 306 | subplot(311) 307 | plot(t,X_sol(:,1),'g','linewidth',1.5); hold on 308 | legend('Ground Truth','Measurement','MHE') 309 | subplot(312) 310 | plot(t,X_sol(:,2),'g','linewidth',1.5); hold on 311 | subplot(313) 312 | plot(t,X_sol(:,3),'g','linewidth',1.5); hold on 313 | 314 | 315 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MHE_code/MHE_Robot_PS_mul_shooting_v2.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.1.1 7 | % addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | % CasADi v3.5.5 9 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5') 10 | import casadi.* 11 | 12 | T = 0.2; %[s] 13 | N = 10; % prediction horizon 14 | rob_diam = 0.3; 15 | 16 | v_max = 0.6; v_min = -v_max; 17 | omega_max = pi/4; omega_min = -omega_max; 18 | 19 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 20 | states = [x;y;theta]; n_states = length(states); 21 | 22 | v = SX.sym('v'); omega = SX.sym('omega'); 23 | controls = [v;omega]; n_controls = length(controls); 24 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 25 | 26 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 27 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 28 | P = SX.sym('P',n_states + n_states); 29 | % parameters (which include at the initial state of the robot and the reference state) 30 | 31 | X = SX.sym('X',n_states,(N+1)); 32 | % A vector that represents the states over the optimization problem. 33 | 34 | obj = 0; % Objective function 35 | g = []; % constraints vector 36 | 37 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 38 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 39 | 40 | st = X(:,1); % initial state 41 | g = [g;st-P(1:3)]; % initial condition constraints 42 | for k = 1:N 43 | st = X(:,k); con = U(:,k); 44 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 45 | st_next = X(:,k+1); 46 | f_value = f(st,con); 47 | st_next_euler = st+ (T*f_value); 48 | g = [g;st_next-st_next_euler]; % compute constraints 49 | end 50 | % make the decision variable one column vector 51 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 52 | 53 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 54 | 55 | opts = struct; 56 | opts.ipopt.max_iter = 2000; 57 | opts.ipopt.print_level =0;%0,3 58 | opts.print_time = 0; 59 | opts.ipopt.acceptable_tol =1e-8; 60 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 61 | 62 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 63 | 64 | 65 | args = struct; 66 | 67 | args.lbg(1:3*(N+1)) = 0; 68 | args.ubg(1:3*(N+1)) = 0; 69 | 70 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 71 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 72 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 73 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 74 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 75 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 76 | 77 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 78 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 79 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 80 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 81 | %---------------------------------------------- 82 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 83 | 84 | 85 | % THE SIMULATION LOOP SHOULD START FROM HERE 86 | %------------------------------------------- 87 | t0 = 0; 88 | x0 = [0.1 ; 0.1 ; 0.0]; % initial condition. 89 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 90 | 91 | xx(:,1) = x0; % xx contains the history of states 92 | t(1) = t0; 93 | 94 | u0 = zeros(N,2); % two control inputs for each robot 95 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 96 | 97 | sim_tim = 20; % total sampling times 98 | 99 | % Start MPC 100 | mpciter = 0; 101 | xx1 = []; 102 | u_cl=[]; 103 | 104 | % the main simulaton loop... it works as long as the error is greater 105 | % than 10^-6 and the number of mpc steps is less than its maximum 106 | % value. 107 | tic 108 | while(norm((x0-xs),2) > 0.05 && mpciter < sim_tim / T) 109 | args.p = [x0;xs]; % set the values of the parameters vector 110 | % initial value of the optimization variables 111 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 112 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 113 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 114 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 115 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 116 | u_cl= [u_cl ; u(1,:)]; 117 | t(mpciter+1) = t0; 118 | % Apply the control and shift the solution 119 | [t0, x0, u0] = shift(T, t0, x0, u,f); 120 | xx(:,mpciter+2) = x0; 121 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 122 | % Shift trajectory to initialize the next step 123 | X0 = [X0(2:end,:);X0(end,:)]; 124 | mpciter 125 | mpciter = mpciter + 1; 126 | end; 127 | toc 128 | 129 | ss_error = norm((x0-xs),2) 130 | %Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 131 | %----------------------------------------- 132 | %----------------------------------------- 133 | %----------------------------------------- 134 | % Start MHE implementation from here 135 | %----------------------------------------- 136 | %----------------------------------------- 137 | %----------------------------------------- 138 | % plot the ground truth 139 | figure(1) 140 | subplot(311) 141 | plot(t,xx(1,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 142 | ylabel('x (m)') 143 | grid on 144 | subplot(312) 145 | plot(t,xx(2,1:end-1),'b','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 146 | ylabel('y (m)') 147 | grid on 148 | subplot(313) 149 | plot(t,xx(3,1:end-1),'b','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 150 | xlabel('time (seconds)') 151 | ylabel('\theta (rad)') 152 | grid on 153 | 154 | % Synthesize the measurments 155 | con_cov = diag([0.005 deg2rad(2)]).^2; 156 | meas_cov = diag([0.1 deg2rad(2)]).^2; 157 | 158 | r = []; 159 | alpha = []; 160 | for k = 1: length(xx(1,:))-1 161 | r = [r; sqrt(xx(1,k)^2+xx(2,k)^2) + sqrt(meas_cov(1,1))*randn(1)]; 162 | alpha = [alpha; atan(xx(2,k)/xx(1,k)) + sqrt(meas_cov(2,2))*randn(1)]; 163 | end 164 | y_measurements = [ r , alpha ]; 165 | 166 | % Plot the cartesian coordinates from the measurements used 167 | figure(1) 168 | subplot(311) 169 | plot(t,r.*cos(alpha),'r','linewidth',1.5); hold on 170 | grid on 171 | legend('Ground Truth','Measurement') 172 | subplot(312) 173 | plot(t,r.*sin(alpha),'r','linewidth',1.5); hold on 174 | grid on 175 | 176 | % plot the ground truth mesurements VS the noisy measurements 177 | figure(2) 178 | subplot(211) 179 | plot(t,sqrt(xx(1,1:end-1).^2+xx(2,1:end-1).^2),'b','linewidth',1.5); hold on 180 | plot(t,r,'r','linewidth',1.5); axis([0 t(end) -0.2 3]); hold on 181 | ylabel('Range: [ r (m) ]') 182 | grid on 183 | legend('Ground Truth','Measurement') 184 | subplot(212) 185 | plot(t,atan(xx(2,1:end-1)./xx(1,1:end-1)),'b','linewidth',1.5); hold on 186 | plot(t,alpha,'r','linewidth',1.5); axis([0 t(end) 0.2 1]); hold on 187 | ylabel('Bearing: [ \alpha (rad) ]') 188 | grid on 189 | 190 | % The following two matrices contain what we know about the system, i.e. 191 | % the nominal control actions applied (measured) and the range and bearing 192 | % measurements. 193 | %------------------------------------------------------------------------- 194 | u_cl; 195 | y_measurements; 196 | 197 | % Let's now formulate our MHE problem 198 | %------------------------------------ 199 | T = 0.2; %[s] 200 | N_MHE = 6; % prediction horizon 201 | 202 | v_max = 0.6; v_min = -v_max; 203 | omega_max = pi/4; omega_min = -omega_max; 204 | 205 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 206 | states = [x;y;theta]; n_states = length(states); 207 | v = SX.sym('v'); omega = SX.sym('omega'); 208 | controls = [v;omega]; n_controls = length(controls); 209 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 210 | f = Function('f',{states,controls},{rhs}); % MOTION MODEL 211 | 212 | r = SX.sym('r'); alpha = SX.sym('alpha'); % range and bearing 213 | measurement_rhs = [sqrt(x^2+y^2); atan(y/x)]; 214 | h = Function('h',{states},{measurement_rhs}); % MEASUREMENT MODEL 215 | %y_tilde = [r;alpha]; 216 | 217 | % Decision variables 218 | U = SX.sym('U',n_controls,N_MHE); %(controls) 219 | X = SX.sym('X',n_states,(N_MHE+1)); %(states) [remember multiple shooting] 220 | 221 | P = SX.sym('P', 2 , N_MHE + (N_MHE+1)); 222 | % parameters (include r and alpha measurements as well as controls measurements) 223 | 224 | V = inv(sqrt(meas_cov)); % weighing matrices (output) y_tilde - y 225 | W = inv(sqrt(con_cov)); % weighing matrices (input) u_tilde - u 226 | 227 | obj = 0; % Objective function 228 | g = []; % constraints vector 229 | for k = 1:N_MHE+1 230 | st = X(:,k); 231 | h_x = h(st); 232 | y_tilde = P(:,k); 233 | obj = obj+ (y_tilde-h_x)' * V * (y_tilde-h_x); % calculate obj 234 | end 235 | 236 | for k = 1:N_MHE 237 | con = U(:,k); 238 | u_tilde = P(:,N_MHE+ k); 239 | obj = obj+ (u_tilde-con)' * W * (u_tilde-con); % calculate obj 240 | end 241 | 242 | % multiple shooting constraints 243 | for k = 1:N_MHE 244 | st = X(:,k); con = U(:,k); 245 | st_next = X(:,k+1); 246 | f_value = f(st,con); 247 | st_next_euler = st+ (T*f_value); 248 | g = [g;st_next-st_next_euler]; % compute constraints 249 | end 250 | 251 | 252 | % make the decision variable one column vector 253 | OPT_variables = [reshape(X,3*(N_MHE+1),1);reshape(U,2*N_MHE,1)]; 254 | 255 | nlp_mhe = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 256 | 257 | opts = struct; 258 | opts.ipopt.max_iter = 2000; 259 | opts.ipopt.print_level =0;%0,3 260 | opts.print_time = 0; 261 | opts.ipopt.acceptable_tol =1e-8; 262 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 263 | 264 | solver = nlpsol('solver', 'ipopt', nlp_mhe,opts); 265 | 266 | 267 | args = struct; 268 | 269 | args.lbg(1:3*(N_MHE)) = 0; 270 | args.ubg(1:3*(N_MHE)) = 0; 271 | 272 | args.lbx(1:3:3*(N_MHE+1),1) = -2; %state x lower bound 273 | args.ubx(1:3:3*(N_MHE+1),1) = 2; %state x upper bound 274 | args.lbx(2:3:3*(N_MHE+1),1) = -2; %state y lower bound 275 | args.ubx(2:3:3*(N_MHE+1),1) = 2; %state y upper bound 276 | args.lbx(3:3:3*(N_MHE+1),1) = -pi/2; %state theta lower bound 277 | args.ubx(3:3:3*(N_MHE+1),1) = pi/2; %state theta upper bound 278 | 279 | args.lbx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_min; %v lower bound 280 | args.ubx(3*(N_MHE+1)+1:2:3*(N_MHE+1)+2*N_MHE,1) = v_max; %v upper bound 281 | args.lbx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_min; %omega lower bound 282 | args.ubx(3*(N_MHE+1)+2:2:3*(N_MHE+1)+2*N_MHE,1) = omega_max; %omega upper bound 283 | %---------------------------------------------- 284 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 285 | 286 | % MHE Simulation loop starts here 287 | %------------------------------------------ 288 | X_estimate = []; % X_estimate contains the MHE estimate of the states 289 | U_estimate = []; % U_estimate contains the MHE estimate of the controls 290 | 291 | U0 = zeros(N_MHE,2); % two control inputs for each robot 292 | X0 = zeros(N_MHE+1,3); % initialization of the states decision variables 293 | 294 | % Start MHE 295 | mheiter = 0; 296 | 297 | U0 = u_cl(1:N_MHE,:); % initialize the control actions by the measured 298 | % initialize the states from the measured range and bearing 299 | X0(:,1:2) = [y_measurements(1:N_MHE+1,1).*cos(y_measurements(1:N_MHE+1,2)),... 300 | y_measurements(1:N_MHE+1,1).*sin(y_measurements(1:N_MHE+1,2))]; 301 | 302 | tic 303 | for k = 1: size(y_measurements,1) - (N_MHE) 304 | mheiter = k 305 | % Get the measurements window and put it as parameters in p 306 | args.p = [y_measurements(k:k+N_MHE,:)',u_cl(k:k+N_MHE-1,:)']; 307 | % initial value of the optimization variables 308 | args.x0 = [reshape(X0',3*(N_MHE+1),1);reshape(U0',2*N_MHE,1)]; 309 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 310 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 311 | U_sol = reshape(full(sol.x(3*(N_MHE+1)+1:end))',2,N_MHE)'; % get controls only from the solution 312 | X_sol = reshape(full(sol.x(1:3*(N_MHE+1)))',3,N_MHE+1)'; % get solution TRAJECTORY 313 | X_estimate = [X_estimate;X_sol(N_MHE+1,:)]; 314 | U_estimate = [U_estimate;U_sol(N_MHE,:)]; 315 | 316 | % Shift trajectory to initialize the next step 317 | X0 = [X_sol(2:end,:);X_sol(end,:)]; 318 | U0 = [U_sol(2:end,:);U_sol(end,:)]; 319 | end; 320 | toc 321 | 322 | figure(1) 323 | subplot(311) 324 | plot(t(N_MHE+1:end),X_estimate(:,1),'g','linewidth',1.5); hold on 325 | legend('Ground Truth','Measurement','MHE') 326 | 327 | subplot(312) 328 | plot(t(N_MHE+1:end),X_estimate(:,2),'g','linewidth',1.5); axis([0 t(end) 0 1.8]);hold on 329 | 330 | subplot(313) 331 | plot(t(N_MHE+1:end),X_estimate(:,3),'g','linewidth',1.5); axis([0 t(end) -pi/4 pi/2]);hold on 332 | 333 | 334 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MHE_code/shift.m: -------------------------------------------------------------------------------- 1 | function [t0, x0, u0] = shift(T, t0, x0, u,f) 2 | % add noise to the control actions before applying it 3 | con_cov = diag([0.005 deg2rad(2)]).^2; 4 | con = u(1,:)' + sqrt(con_cov)*randn(2,1); 5 | st = x0; 6 | 7 | f_value = f(st,con); 8 | st = st+ (T*f_value); 9 | 10 | x0 = full(st); 11 | t0 = t0 + T; 12 | 13 | u0 = [u(2:size(u,1),:);u(size(u,1),:)]; % shift the control action 14 | end -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Draw_MPC_PS_Obstacles.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_PS_Obstacles (t,xx,xx1,u_cl,xs,N,rob_diam,obs_x,obs_y,obs_diam) 2 | 3 | set(0,'DefaultAxesFontName', 'Times New Roman') 4 | set(0,'DefaultAxesFontSize', 12) 5 | 6 | line_width = 1.5; 7 | fontsize_labels = 14; 8 | 9 | %-------------------------------------------------------------------------- 10 | %-----------------------Simulate robots ----------------------------------- 11 | %-------------------------------------------------------------------------- 12 | x_r_1 = []; 13 | y_r_1 = []; 14 | 15 | 16 | 17 | r = rob_diam/2; % obstacle radius 18 | ang=0:0.005:2*pi; 19 | xp=r*cos(ang); 20 | yp=r*sin(ang); 21 | 22 | r = obs_diam/2; % obstacle radius 23 | xp_obs=r*cos(ang); 24 | yp_obs=r*sin(ang); 25 | 26 | figure(500) 27 | % Animate the robot motion 28 | %figure;%('Position',[200 200 1280 720]); 29 | set(gcf,'PaperPositionMode','auto') 30 | set(gcf, 'Color', 'w'); 31 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 32 | 33 | tic 34 | for k = 1:size(xx,2) 35 | h_t = 0.14; w_t=0.09; % triangle parameters 36 | 37 | x1 = xs(1); y1 = xs(2); th1 = xs(3); 38 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 39 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 40 | fill(x1_tri, y1_tri, 'g'); % plot reference state 41 | hold on; 42 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 43 | x_r_1 = [x_r_1 x1]; 44 | y_r_1 = [y_r_1 y1]; 45 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 46 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 47 | 48 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 49 | if k < size(xx,2) % plot prediction 50 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 51 | for j = 2:N+1 52 | plot(xx1(j,1,k)+xp,xx1(j,2,k)+yp,'--r'); % plot robot circle 53 | end 54 | end 55 | 56 | fill(x1_tri, y1_tri, 'r'); % plot robot position 57 | 58 | plot(x1+xp,y1+yp,'--r'); % plot robot circle 59 | 60 | plot(obs_x+xp_obs,obs_y+yp_obs,'--b'); % plot robot circle 61 | 62 | hold off 63 | %figure(500) 64 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 65 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 66 | axis([-0.4 1.8 -0.2 1.8]) 67 | pause(0.1) 68 | box on; 69 | grid on 70 | %aviobj = addframe(aviobj,gcf); 71 | drawnow 72 | % for video generation 73 | F(k) = getframe(gcf); % to get the current frame 74 | end 75 | toc 76 | close(gcf) 77 | %viobj = close(aviobj) 78 | video = VideoWriter('exp.avi','Uncompressed AVI'); 79 | 80 | video = VideoWriter('exp.avi','Motion JPEG AVI'); 81 | video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 82 | open(video) 83 | writeVideo(video,F) 84 | close (video) 85 | 86 | figure 87 | subplot(211) 88 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.75 0.75]) 89 | ylabel('v (rad/s)') 90 | grid on 91 | subplot(212) 92 | stairs(t,u_cl(:,2),'r','linewidth',1.5); axis([0 t(end) -0.85 0.85]) 93 | xlabel('time (seconds)') 94 | ylabel('\omega (rad/s)') 95 | grid on 96 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Draw_MPC_point_stabilization_v1.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 2 | 3 | 4 | set(0,'DefaultAxesFontName', 'Times New Roman') 5 | set(0,'DefaultAxesFontSize', 12) 6 | 7 | line_width = 1.5; 8 | fontsize_labels = 14; 9 | 10 | %-------------------------------------------------------------------------- 11 | %-----------------------Simulate robots ----------------------------------- 12 | %-------------------------------------------------------------------------- 13 | x_r_1 = []; 14 | y_r_1 = []; 15 | 16 | 17 | 18 | r = rob_diam/2; % obstacle radius 19 | ang=0:0.005:2*pi; 20 | xp=r*cos(ang); 21 | yp=r*sin(ang); 22 | 23 | figure(500) 24 | % Animate the robot motion 25 | %figure;%('Position',[200 200 1280 720]); 26 | set(gcf,'PaperPositionMode','auto') 27 | set(gcf, 'Color', 'w'); 28 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 29 | 30 | for k = 1:size(xx,2) 31 | h_t = 0.14; w_t=0.09; % triangle parameters 32 | 33 | x1 = xs(1); y1 = xs(2); th1 = xs(3); 34 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 35 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 36 | fill(x1_tri, y1_tri, 'g'); % plot reference state 37 | hold on; 38 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 39 | x_r_1 = [x_r_1 x1]; 40 | y_r_1 = [y_r_1 y1]; 41 | x1_tri = [ x1+h_t*cos(th1), x1+(w_t/2)*cos((pi/2)-th1), x1-(w_t/2)*cos((pi/2)-th1)];%,x1+(h_t/3)*cos(th1)]; 42 | y1_tri = [ y1+h_t*sin(th1), y1-(w_t/2)*sin((pi/2)-th1), y1+(w_t/2)*sin((pi/2)-th1)];%,y1+(h_t/3)*sin(th1)]; 43 | 44 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 45 | if k < size(xx,2) % plot prediction 46 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 47 | end 48 | 49 | fill(x1_tri, y1_tri, 'r'); % plot robot position 50 | plot(x1+xp,y1+yp,'--r'); % plot robot circle 51 | 52 | 53 | hold off 54 | %figure(500) 55 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 56 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 57 | axis([-0.2 1.8 -0.2 1.8]) 58 | pause(0.1) 59 | box on; 60 | grid on 61 | %aviobj = addframe(aviobj,gcf); 62 | drawnow 63 | % for video generation 64 | F(k) = getframe(gcf); % to get the current frame 65 | end 66 | close(gcf) 67 | %viobj = close(aviobj) 68 | %video = VideoWriter('exp.avi','Uncompressed AVI'); 69 | 70 | % video = VideoWriter('exp.avi','Motion JPEG AVI'); 71 | % video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 72 | % open(video) 73 | % writeVideo(video,F) 74 | % close (video) 75 | 76 | figure 77 | subplot(211) 78 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.35 0.75]) 79 | ylabel('v (rad/s)') 80 | grid on 81 | subplot(212) 82 | stairs(t,u_cl(:,2),'r','linewidth',1.5); axis([0 t(end) -0.85 0.85]) 83 | xlabel('time (seconds)') 84 | ylabel('\omega (rad/s)') 85 | grid on 86 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Draw_MPC_tracking_v1.m: -------------------------------------------------------------------------------- 1 | function Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 2 | 3 | 4 | set(0,'DefaultAxesFontName', 'Times New Roman') 5 | set(0,'DefaultAxesFontSize', 12) 6 | 7 | line_width = 1.5; 8 | fontsize_labels = 14; 9 | 10 | %-------------------------------------------------------------------------- 11 | %-----------------------Simulate robots ----------------------------------- 12 | %-------------------------------------------------------------------------- 13 | x_r_1 = []; 14 | y_r_1 = []; 15 | 16 | r = rob_diam/2; % obstacle radius 17 | ang=0:0.005:2*pi; 18 | xp=r*cos(ang); 19 | yp=r*sin(ang); 20 | 21 | figure(500) 22 | % Animate the robot motion 23 | %figure;%('Position',[200 200 1280 720]); 24 | set(gcf,'PaperPositionMode','auto') 25 | set(gcf, 'Color', 'w'); 26 | set(gcf,'Units','normalized','OuterPosition',[0 0 0.55 1]); 27 | 28 | for k = 1:size(xx,2) 29 | plot([0 12],[1 1],'-g','linewidth',1.2);hold on % plot the reference trajectory 30 | x1 = xx(1,k,1); y1 = xx(2,k,1); th1 = xx(3,k,1); 31 | x_r_1 = [x_r_1 x1]; 32 | y_r_1 = [y_r_1 y1]; 33 | plot(x_r_1,y_r_1,'-r','linewidth',line_width);hold on % plot exhibited trajectory 34 | if k < size(xx,2) % plot prediction 35 | plot(xx1(1:N,1,k),xx1(1:N,2,k),'r--*') 36 | end 37 | 38 | plot(x1,y1,'-sk','MarkerSize',25)% plot robot position 39 | hold off 40 | %figure(500) 41 | ylabel('$y$-position (m)','interpreter','latex','FontSize',fontsize_labels) 42 | xlabel('$x$-position (m)','interpreter','latex','FontSize',fontsize_labels) 43 | axis([-1 16 -0.5 1.5]) 44 | pause(0.2) 45 | box on; 46 | grid on 47 | %aviobj = addframe(aviobj,gcf); 48 | drawnow 49 | % for video generation 50 | F(k) = getframe(gcf); % to get the current frame 51 | end 52 | close(gcf) 53 | %viobj = close(aviobj) 54 | %video = VideoWriter('exp.avi','Uncompressed AVI'); 55 | 56 | % video = VideoWriter('exp.avi','Motion JPEG AVI'); 57 | % video.FrameRate = 5; % (frames per second) this number depends on the sampling time and the number of frames you have 58 | % open(video) 59 | % writeVideo(video,F) 60 | % close (video) 61 | 62 | figure 63 | subplot(211) 64 | stairs(t,u_cl(:,1),'k','linewidth',1.5); axis([0 t(end) -0.2 0.8]) 65 | ylabel('v (rad/s)') 66 | grid on 67 | subplot(212) 68 | stairs(t,u_cl(:,2),'r','linewidth',1.5); %axis([0 t(end) -0.85 0.85]) 69 | xlabel('time (seconds)') 70 | ylabel('\omega (rad/s)') 71 | grid on -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Sim_1_MPC_Robot_PS_sing_shooting.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Single shooting 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | % addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | % CasADi v3.5.5 9 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5') 10 | import casadi.* 11 | 12 | T = 0.2; % sampling time [s] 13 | N = 20; % prediction horizon 14 | rob_diam = 0.3; 15 | 16 | v_max = 0.6; v_min = -v_max; 17 | omega_max = pi/4; omega_min = -omega_max; 18 | 19 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 20 | states = [x;y;theta]; n_states = length(states); 21 | 22 | v = SX.sym('v'); omega = SX.sym('omega'); 23 | controls = [v;omega]; n_controls = length(controls); 24 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 25 | 26 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 27 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 28 | P = SX.sym('P',n_states + n_states); 29 | % parameters (which include the initial and the reference state of the robot) 30 | 31 | X = SX.sym('X',n_states,(N+1)); 32 | % A Matrix that represents the states over the optimization problem. 33 | 34 | % compute solution symbolically 35 | X(:,1) = P(1:3); % initial state 36 | for k = 1:N 37 | st = X(:,k); con = U(:,k); 38 | f_value = f(st,con); 39 | st_next = st+ (T*f_value); 40 | X(:,k+1) = st_next; 41 | end 42 | % this function to get the optimal trajectory knowing the optimal solution 43 | ff=Function('ff',{U,P},{X}); 44 | 45 | obj = 0; % Objective function 46 | g = []; % constraints vector 47 | 48 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 49 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 50 | % compute objective 51 | for k=1:N 52 | st = X(:,k); con = U(:,k); 53 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 54 | end 55 | 56 | % compute constraints 57 | for k = 1:N+1 % box constraints due to the map margins 58 | g = [g ; X(1,k)]; %state x 59 | g = [g ; X(2,k)]; %state y 60 | end 61 | 62 | % make the decision variables one column vector 63 | OPT_variables = reshape(U,2*N,1); 64 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 65 | 66 | opts = struct; 67 | opts.ipopt.max_iter = 100; 68 | opts.ipopt.print_level =0;%0,3 69 | opts.print_time = 0; 70 | opts.ipopt.acceptable_tol =1e-8; 71 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 72 | 73 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 74 | 75 | 76 | args = struct; 77 | % inequality constraints (state constraints) 78 | args.lbg = -2; % lower bound of the states x and y 79 | args.ubg = 2; % upper bound of the states x and y 80 | 81 | % input constraints 82 | args.lbx(1:2:2*N-1,1) = v_min; args.lbx(2:2:2*N,1) = omega_min; 83 | args.ubx(1:2:2*N-1,1) = v_max; args.ubx(2:2:2*N,1) = omega_max; 84 | 85 | 86 | %---------------------------------------------- 87 | % ALL OF THE ABOVE IS JUST A PROBLEM SETTING UP 88 | 89 | 90 | % THE SIMULATION LOOP SHOULD START FROM HERE 91 | %------------------------------------------- 92 | t0 = 0; 93 | x0 = [0 ; 0 ; 0.0]; % initial condition. 94 | xs = [1.5 ; 1.5 ; 0]; % Reference posture. 95 | 96 | xx(:,1) = x0; % xx contains the history of states 97 | t(1) = t0; 98 | 99 | u0 = zeros(N,2); % two control inputs 100 | 101 | sim_tim = 20; % Maximum simulation time 102 | 103 | % Start MPC 104 | mpciter = 0; 105 | xx1 = []; 106 | u_cl=[]; 107 | 108 | 109 | % the main simulaton loop... it works as long as the error is greater 110 | % than 10^-2 and the number of mpc steps is less than its maximum 111 | % value. 112 | main_loop = tic; 113 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 114 | args.p = [x0;xs]; % set the values of the parameters vector 115 | args.x0 = reshape(u0',2*N,1); % initial value of the optimization variables 116 | %tic 117 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 118 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 119 | %toc 120 | u = reshape(full(sol.x)',2,N)'; 121 | ff_value = ff(u',args.p); % compute OPTIMAL solution TRAJECTORY 122 | xx1(:,1:3,mpciter+1)= full(ff_value)'; 123 | 124 | u_cl= [u_cl ; u(1,:)]; 125 | t(mpciter+1) = t0; 126 | [t0, x0, u0] = shift(T, t0, x0, u,f); % get the initialization of the next optimization step 127 | 128 | xx(:,mpciter+2) = x0; 129 | mpciter 130 | mpciter = mpciter + 1; 131 | end; 132 | main_loop_time = toc(main_loop) 133 | 134 | ss_error = norm((x0-xs),2) 135 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) % a drawing function 136 | 137 | 138 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Sim_2_MPC_R_PS_mul_shooting_RK.m: -------------------------------------------------------------------------------- 1 | % point stabilization + Multiple shooting + Runge Kutta 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.5.1 7 | % addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.1') 8 | % CasADi v3.5.5 9 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5') 10 | import casadi.* 11 | 12 | h = 0.2; %[s] 13 | N = 10; % prediction horizon 14 | rob_diam = 0.3; 15 | 16 | v_max = 0.6; v_min = -v_max; 17 | omega_max = pi/4; omega_min = -omega_max; 18 | 19 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 20 | states = [x;y;theta]; n_states = length(states); 21 | 22 | v = SX.sym('v'); omega = SX.sym('omega'); 23 | controls = [v;omega]; n_controls = length(controls); 24 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 25 | 26 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 27 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 28 | P = SX.sym('P',n_states + n_states); 29 | % parameters (which include the initial state and the reference state) 30 | 31 | X = SX.sym('X',n_states,(N+1)); 32 | % A vector that represents the states over the optimization problem. 33 | 34 | obj = 0; % Objective function 35 | g = []; % constraints vector 36 | 37 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 38 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 39 | 40 | st = X(:,1); % initial state 41 | g = [g;st-P(1:3)]; % initial condition constraints 42 | for k = 1:N 43 | st = X(:,k); con = U(:,k); 44 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 45 | st_next = X(:,k+1); 46 | k1 = f(st, con); % new 47 | k2 = f(st + h/2*k1, con); % new 48 | k3 = f(st + h/2*k2, con); % new 49 | k4 = f(st + h*k3, con); % new 50 | st_next_RK4=st +h/6*(k1 +2*k2 +2*k3 +k4); % new 51 | % f_value = f(st,con); 52 | % st_next_euler = st+ (h*f_value); 53 | % g = [g;st_next-st_next_euler]; % compute constraints 54 | g = [g;st_next-st_next_RK4]; % compute constraints % new 55 | end 56 | % make the decision variable one column vector 57 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 58 | 59 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 60 | 61 | opts = struct; 62 | opts.ipopt.max_iter = 2000; 63 | opts.ipopt.print_level =0;%0,3 64 | opts.print_time = 0; 65 | opts.ipopt.acceptable_tol =1e-8; 66 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 67 | 68 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 69 | 70 | args = struct; 71 | 72 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 73 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 74 | 75 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 76 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 77 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 78 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 79 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 80 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 81 | 82 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 83 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 84 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 85 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 86 | %---------------------------------------------- 87 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 88 | 89 | 90 | % THE SIMULATION LOOP SHOULD START FROM HERE 91 | %------------------------------------------- 92 | t0 = 0; 93 | x0 = [0 ; 0 ; 0.0]; % initial condition. 94 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 95 | 96 | xx(:,1) = x0; % xx contains the history of states 97 | t(1) = t0; 98 | 99 | u0 = zeros(N,2); % two control inputs for each robot 100 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 101 | 102 | sim_tim = 20; % Maximum simulation time 103 | 104 | % Start MPC 105 | mpciter = 0; 106 | xx1 = []; 107 | u_cl=[]; 108 | 109 | % the main simulaton loop... it works as long as the error is greater 110 | % than 10^-6 and the number of mpc steps is less than its maximum 111 | % value. 112 | main_loop = tic; 113 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / h) 114 | args.p = [x0;xs]; % set the values of the parameters vector 115 | % initial value of the optimization variables 116 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 117 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 118 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 119 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 120 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 121 | u_cl= [u_cl ; u(1,:)]; 122 | t(mpciter+1) = t0; 123 | % Apply the control and shift the solution 124 | [t0, x0, u0] = shift(h, t0, x0, u,f); 125 | xx(:,mpciter+2) = x0; 126 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 127 | % Shift trajectory to initialize the next step 128 | X0 = [X0(2:end,:);X0(end,:)]; 129 | mpciter 130 | mpciter = mpciter + 1; 131 | end; 132 | main_loop_time = toc(main_loop); 133 | ss_error = norm((x0-xs),2) 134 | average_mpc_time = main_loop_time/(mpciter+1) 135 | 136 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 137 | 138 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Sim_2_MPC_Robot_PS_mul_shooting.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | % addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | % CasADi v3.5.5 9 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5') 10 | 11 | import casadi.* 12 | 13 | T = 0.2; %[s] 14 | N = 10; % prediction horizon 15 | rob_diam = 0.3; 16 | 17 | v_max = 0.6; v_min = -v_max; 18 | omega_max = pi/4; omega_min = -omega_max; 19 | 20 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 21 | states = [x;y;theta]; n_states = length(states); 22 | 23 | v = SX.sym('v'); omega = SX.sym('omega'); 24 | controls = [v;omega]; n_controls = length(controls); 25 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 26 | 27 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 28 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 29 | P = SX.sym('P',n_states + n_states); 30 | % parameters (which include the initial state and the reference state) 31 | 32 | X = SX.sym('X',n_states,(N+1)); 33 | % A vector that represents the states over the optimization problem. 34 | 35 | obj = 0; % Objective function 36 | g = []; % constraints vector 37 | 38 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 39 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 40 | 41 | st = X(:,1); % initial state 42 | g = [g;st-P(1:3)]; % initial condition constraints 43 | for k = 1:N 44 | st = X(:,k); con = U(:,k); 45 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 46 | st_next = X(:,k+1); 47 | f_value = f(st,con); 48 | st_next_euler = st+ (T*f_value); 49 | g = [g;st_next-st_next_euler]; % compute constraints 50 | end 51 | % make the decision variable one column vector 52 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 53 | 54 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 55 | 56 | opts = struct; 57 | opts.ipopt.max_iter = 2000; 58 | opts.ipopt.print_level =0;%0,3 59 | opts.print_time = 0; 60 | opts.ipopt.acceptable_tol =1e-8; 61 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 62 | 63 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 64 | 65 | args = struct; 66 | 67 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 68 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 69 | 70 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 71 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 72 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 73 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 74 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 75 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 76 | 77 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 78 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 79 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 80 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 81 | %---------------------------------------------- 82 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 83 | 84 | 85 | % THE SIMULATION LOOP SHOULD START FROM HERE 86 | %------------------------------------------- 87 | t0 = 0; 88 | x0 = [0 ; 0 ; 0.0]; % initial condition. 89 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 90 | 91 | xx(:,1) = x0; % xx contains the history of states 92 | t(1) = t0; 93 | 94 | u0 = zeros(N,2); % two control inputs for each robot 95 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 96 | 97 | sim_tim = 20; % Maximum simulation time 98 | 99 | % Start MPC 100 | mpciter = 0; 101 | xx1 = []; 102 | u_cl=[]; 103 | 104 | % the main simulaton loop... it works as long as the error is greater 105 | % than 10^-6 and the number of mpc steps is less than its maximum 106 | % value. 107 | tic 108 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 109 | args.p = [x0;xs]; % set the values of the parameters vector 110 | % initial value of the optimization variables 111 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 112 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 113 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 114 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 115 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 116 | u_cl= [u_cl ; u(1,:)]; 117 | t(mpciter+1) = t0; 118 | % Apply the control and shift the solution 119 | [t0, x0, u0] = shift(T, t0, x0, u,f); 120 | xx(:,mpciter+2) = x0; 121 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 122 | % Shift trajectory to initialize the next step 123 | X0 = [X0(2:end,:);X0(end,:)]; 124 | mpciter 125 | mpciter = mpciter + 1; 126 | end; 127 | toc 128 | 129 | ss_error = norm((x0-xs),2) 130 | Draw_MPC_point_stabilization_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 131 | 132 | 133 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Sim_3_MPC_Robot_PS_obs_avoid_mul_sh.m: -------------------------------------------------------------------------------- 1 | % first casadi test for mpc fpr mobile robots 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | % addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | % CasADi v3.5.5 9 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5') 10 | 11 | import casadi.* 12 | 13 | T = 0.2; %[s] 14 | N = 14; % prediction horizon 15 | rob_diam = 0.3; 16 | 17 | v_max = 0.6; v_min = -v_max; 18 | omega_max = pi/4; omega_min = -omega_max; 19 | 20 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 21 | states = [x;y;theta]; n_states = length(states); 22 | 23 | v = SX.sym('v'); omega = SX.sym('omega'); 24 | controls = [v;omega]; n_controls = length(controls); 25 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 26 | 27 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 28 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 29 | P = SX.sym('P',n_states + n_states); 30 | % parameters (which include at the initial state of the robot and the reference state) 31 | 32 | X = SX.sym('X',n_states,(N+1)); 33 | % A vector that represents the states over the optimization problem. 34 | 35 | obj = 0; % Objective function 36 | g = []; % constraints vector 37 | 38 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 5;Q(3,3) = 0.1; % weighing matrices (states) 39 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 40 | 41 | st = X(:,1); % initial state 42 | g = [g;st-P(1:3)]; % initial condition constraints 43 | for k = 1:N 44 | st = X(:,k); con = U(:,k); 45 | obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 46 | st_next = X(:,k+1); 47 | f_value = f(st,con); 48 | st_next_euler = st+ (T*f_value); 49 | g = [g;st_next-st_next_euler]; % compute constraints 50 | end 51 | % Add constraints for collision avoidance 52 | obs_x = 0.5; % meters 53 | obs_y = 0.5; % meters 54 | obs_diam = 0.3; % meters 55 | for k = 1:N+1 % box constraints due to the map margins 56 | g = [g ; -sqrt((X(1,k)-obs_x)^2+(X(2,k)-obs_y)^2) + (rob_diam/2 + obs_diam/2)]; 57 | end 58 | % make the decision variable one column vector 59 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 60 | 61 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 62 | 63 | opts = struct; 64 | opts.ipopt.max_iter = 100; 65 | opts.ipopt.print_level =0;%0,3 66 | opts.print_time = 0; 67 | opts.ipopt.acceptable_tol =1e-8; 68 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 69 | 70 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 71 | 72 | args = struct; 73 | args.lbg(1:3*(N+1)) = 0; % equality constraints 74 | args.ubg(1:3*(N+1)) = 0; % equality constraints 75 | 76 | args.lbg(3*(N+1)+1 : 3*(N+1)+ (N+1)) = -inf; % inequality constraints 77 | args.ubg(3*(N+1)+1 : 3*(N+1)+ (N+1)) = 0; % inequality constraints 78 | 79 | args.lbx(1:3:3*(N+1),1) = -2; %state x lower bound 80 | args.ubx(1:3:3*(N+1),1) = 2; %state x upper bound 81 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 82 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 83 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 84 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 85 | 86 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 87 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 88 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 89 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 90 | %---------------------------------------------- 91 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 92 | 93 | 94 | % THE SIMULATION LOOP SHOULD START FROM HERE 95 | %------------------------------------------- 96 | t0 = 0; 97 | x0 = [0 ; 0 ; 0.0]; % initial condition. 98 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 99 | 100 | xx(:,1) = x0; % xx contains the history of states 101 | t(1) = t0; 102 | 103 | u0 = zeros(N,2); % two control inputs for each robot 104 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 105 | 106 | sim_tim = 20; % Maximum simulation time 107 | 108 | % Start MPC 109 | mpciter = 0; 110 | xx1 = []; 111 | u_cl=[]; 112 | 113 | % the main simulaton loop... it works as long as the error is greater 114 | % than 10^-6 and the number of mpc steps is less than its maximum 115 | % value. 116 | tic 117 | while(norm((x0-xs),2) > 1e-2 && mpciter < sim_tim / T) 118 | args.p = [x0;xs]; % set the values of the parameters vector 119 | % initial value of the optimization variables 120 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 121 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 122 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 123 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 124 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 125 | u_cl= [u_cl ; u(1,:)]; 126 | t(mpciter+1) = t0; 127 | % Apply the control and shift the solution 128 | [t0, x0, u0] = shift(T, t0, x0, u,f); 129 | xx(:,mpciter+2) = x0; 130 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 131 | % Shift trajectory to initialize the next step 132 | X0 = [X0(2:end,:);X0(end,:)]; 133 | mpciter 134 | mpciter = mpciter + 1; 135 | end; 136 | toc 137 | 138 | ss_error = norm((x0-xs),2) 139 | Draw_MPC_PS_Obstacles (t,xx,xx1,u_cl,xs,N,rob_diam,obs_x,obs_y,obs_diam) 140 | 141 | 142 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/Sim_4_MPC_Robot_tracking_mul_shooting.m: -------------------------------------------------------------------------------- 1 | % Trajectory Tracking + Multiple shooting 2 | clear all 3 | close all 4 | clc 5 | 6 | % CasADi v3.4.5 7 | % addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.4.5') 8 | % CasADi v3.5.5 9 | addpath('C:\Users\mehre\OneDrive\Desktop\CasADi\casadi-windows-matlabR2016a-v3.5.5') 10 | import casadi.* 11 | 12 | T = 0.5; %[s] 13 | N = 8; % prediction horizon 14 | rob_diam = 0.3; 15 | 16 | v_max = 0.6; v_min = -v_max; 17 | omega_max = pi/4; omega_min = -omega_max; 18 | 19 | x = SX.sym('x'); y = SX.sym('y'); theta = SX.sym('theta'); 20 | states = [x;y;theta]; n_states = length(states); 21 | 22 | v = SX.sym('v'); omega = SX.sym('omega'); 23 | controls = [v;omega]; n_controls = length(controls); 24 | rhs = [v*cos(theta);v*sin(theta);omega]; % system r.h.s 25 | 26 | f = Function('f',{states,controls},{rhs}); % nonlinear mapping function f(x,u) 27 | U = SX.sym('U',n_controls,N); % Decision variables (controls) 28 | %P = SX.sym('P',n_states + n_states); 29 | P = SX.sym('P',n_states + N*(n_states+n_controls)); 30 | % parameters (which include the initial state and the reference along the 31 | % predicted trajectory (reference states and reference controls)) 32 | 33 | X = SX.sym('X',n_states,(N+1)); 34 | % A vector that represents the states over the optimization problem. 35 | 36 | obj = 0; % Objective function 37 | g = []; % constraints vector 38 | 39 | Q = zeros(3,3); Q(1,1) = 1;Q(2,2) = 1;Q(3,3) = 0.5; % weighing matrices (states) 40 | R = zeros(2,2); R(1,1) = 0.5; R(2,2) = 0.05; % weighing matrices (controls) 41 | 42 | st = X(:,1); % initial state 43 | g = [g;st-P(1:3)]; % initial condition constraints 44 | for k = 1:N 45 | st = X(:,k); con = U(:,k); 46 | %obj = obj+(st-P(4:6))'*Q*(st-P(4:6)) + con'*R*con; % calculate obj 47 | obj = obj+(st-P(5*k-1:5*k+1))'*Q*(st-P(5*k-1:5*k+1)) + ... 48 | (con-P(5*k+2:5*k+3))'*R*(con-P(5*k+2:5*k+3)) ; % calculate obj 49 | % the number 5 is (n_states+n_controls) 50 | st_next = X(:,k+1); 51 | f_value = f(st,con); 52 | st_next_euler = st+ (T*f_value); 53 | g = [g;st_next-st_next_euler]; % compute constraints 54 | end 55 | % make the decision variable one column vector 56 | OPT_variables = [reshape(X,3*(N+1),1);reshape(U,2*N,1)]; 57 | 58 | nlp_prob = struct('f', obj, 'x', OPT_variables, 'g', g, 'p', P); 59 | 60 | opts = struct; 61 | opts.ipopt.max_iter = 2000; 62 | opts.ipopt.print_level =0;%0,3 63 | opts.print_time = 0; 64 | opts.ipopt.acceptable_tol =1e-8; 65 | opts.ipopt.acceptable_obj_change_tol = 1e-6; 66 | 67 | solver = nlpsol('solver', 'ipopt', nlp_prob,opts); 68 | 69 | args = struct; 70 | 71 | args.lbg(1:3*(N+1)) = 0; % -1e-20 % Equality constraints 72 | args.ubg(1:3*(N+1)) = 0; % 1e-20 % Equality constraints 73 | 74 | args.lbx(1:3:3*(N+1),1) = -20; %state x lower bound % new - adapt the bound 75 | args.ubx(1:3:3*(N+1),1) = 20; %state x upper bound % new - adapt the bound 76 | args.lbx(2:3:3*(N+1),1) = -2; %state y lower bound 77 | args.ubx(2:3:3*(N+1),1) = 2; %state y upper bound 78 | args.lbx(3:3:3*(N+1),1) = -inf; %state theta lower bound 79 | args.ubx(3:3:3*(N+1),1) = inf; %state theta upper bound 80 | 81 | args.lbx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_min; %v lower bound 82 | args.ubx(3*(N+1)+1:2:3*(N+1)+2*N,1) = v_max; %v upper bound 83 | args.lbx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_min; %omega lower bound 84 | args.ubx(3*(N+1)+2:2:3*(N+1)+2*N,1) = omega_max; %omega upper bound 85 | %---------------------------------------------- 86 | % ALL OF THE ABOVE IS JUST A PROBLEM SET UP 87 | 88 | 89 | % THE SIMULATION LOOP SHOULD START FROM HERE 90 | %------------------------------------------- 91 | t0 = 0; 92 | x0 = [0 ; 0 ; 0.0]; % initial condition. 93 | xs = [1.5 ; 1.5 ; 0.0]; % Reference posture. 94 | 95 | xx(:,1) = x0; % xx contains the history of states 96 | t(1) = t0; 97 | 98 | u0 = zeros(N,2); % two control inputs for each robot 99 | X0 = repmat(x0,1,N+1)'; % initialization of the states decision variables 100 | 101 | sim_tim = 30; % Maximum simulation time 102 | 103 | % Start MPC 104 | mpciter = 0; 105 | xx1 = []; 106 | u_cl=[]; 107 | 108 | % the main simulaton loop... it works as long as the error is greater 109 | % than 10^-6 and the number of mpc steps is less than its maximum 110 | % value. 111 | main_loop = tic; 112 | while(mpciter < sim_tim / T) % new - condition for ending the loop 113 | current_time = mpciter*T; %new - get the current time 114 | % args.p = [x0;xs]; % set the values of the parameters vector 115 | %---------------------------------------------------------------------- 116 | args.p(1:3) = x0; % initial condition of the robot posture 117 | for k = 1:N %new - set the reference to track 118 | t_predict = current_time + (k-1)*T; % predicted time instant 119 | x_ref = 0.5*t_predict; y_ref = 1; theta_ref = 0; 120 | u_ref = 0.5; omega_ref = 0; 121 | if x_ref >= 12 % the trajectory end is reached 122 | x_ref = 12; y_ref = 1; theta_ref = 0; 123 | u_ref = 0; omega_ref = 0; 124 | end 125 | args.p(5*k-1:5*k+1) = [x_ref, y_ref, theta_ref]; 126 | args.p(5*k+2:5*k+3) = [u_ref, omega_ref]; 127 | end 128 | %---------------------------------------------------------------------- 129 | % initial value of the optimization variables 130 | args.x0 = [reshape(X0',3*(N+1),1);reshape(u0',2*N,1)]; 131 | sol = solver('x0', args.x0, 'lbx', args.lbx, 'ubx', args.ubx,... 132 | 'lbg', args.lbg, 'ubg', args.ubg,'p',args.p); 133 | u = reshape(full(sol.x(3*(N+1)+1:end))',2,N)'; % get controls only from the solution 134 | xx1(:,1:3,mpciter+1)= reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 135 | u_cl= [u_cl ; u(1,:)]; 136 | t(mpciter+1) = t0; 137 | % Apply the control and shift the solution 138 | [t0, x0, u0] = shift(T, t0, x0, u,f); 139 | xx(:,mpciter+2) = x0; 140 | X0 = reshape(full(sol.x(1:3*(N+1)))',3,N+1)'; % get solution TRAJECTORY 141 | % Shift trajectory to initialize the next step 142 | X0 = [X0(2:end,:);X0(end,:)]; 143 | mpciter 144 | mpciter = mpciter + 1; 145 | end; 146 | main_loop_time = toc(main_loop); 147 | average_mpc_time = main_loop_time/(mpciter+1) 148 | 149 | Draw_MPC_tracking_v1 (t,xx,xx1,u_cl,xs,N,rob_diam) 150 | 151 | -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/exp.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_5_5/MPC_code/exp.avi -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/MPC_code/shift.m: -------------------------------------------------------------------------------- 1 | function [t0, x0, u0] = shift(T, t0, x0, u,f) 2 | st = x0; 3 | con = u(1,:)'; 4 | f_value = f(st,con); 5 | st = st+ (T*f_value); 6 | x0 = full(st); 7 | 8 | t0 = t0 + T; 9 | u0 = [u(2:size(u,1),:);u(size(u,1),:)]; 10 | end -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/motivation_example_1.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_5_5/motivation_example_1.m -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/motivation_example_2.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_5_5/motivation_example_2.m -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/motivation_example_3.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_5_5/motivation_example_3.m -------------------------------------------------------------------------------- /workshop_github/Codes_casadi_v3_5_5/motivation_example_3.xlsx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Codes_casadi_v3_5_5/motivation_example_3.xlsx -------------------------------------------------------------------------------- /workshop_github/MPC_MHE_slides.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/MPC_MHE_slides.pdf -------------------------------------------------------------------------------- /workshop_github/MPC_tracking.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/MPC_tracking.pdf -------------------------------------------------------------------------------- /workshop_github/Python_Implementation/animation1612211110.9228647.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/Python_Implementation/animation1612211110.9228647.gif -------------------------------------------------------------------------------- /workshop_github/Python_Implementation/mpc_code.py: -------------------------------------------------------------------------------- 1 | from time import time 2 | import casadi as ca 3 | import numpy as np 4 | from casadi import sin, cos, pi 5 | import matplotlib.pyplot as plt 6 | from simulation_code import simulate 7 | 8 | # setting matrix_weights' variables 9 | Q_x = 100 10 | Q_y = 100 11 | Q_theta = 2000 12 | R1 = 1 13 | R2 = 1 14 | R3 = 1 15 | R4 = 1 16 | 17 | step_horizon = 0.1 # time between steps in seconds 18 | N = 10 # number of look ahead steps 19 | rob_diam = 0.3 # diameter of the robot 20 | wheel_radius = 1 # wheel radius 21 | Lx = 0.3 # L in J Matrix (half robot x-axis length) 22 | Ly = 0.3 # l in J Matrix (half robot y-axis length) 23 | sim_time = 200 # simulation time 24 | 25 | # specs 26 | x_init = 0 27 | y_init = 0 28 | theta_init = 0 29 | x_target = 15 30 | y_target = 10 31 | theta_target = pi/4 32 | 33 | v_max = 1 34 | v_min = -1 35 | 36 | 37 | def shift_timestep(step_horizon, t0, state_init, u, f): 38 | f_value = f(state_init, u[:, 0]) 39 | next_state = ca.DM.full(state_init + (step_horizon * f_value)) 40 | 41 | t0 = t0 + step_horizon 42 | u0 = ca.horzcat( 43 | u[:, 1:], 44 | ca.reshape(u[:, -1], -1, 1) 45 | ) 46 | 47 | return t0, next_state, u0 48 | 49 | 50 | def DM2Arr(dm): 51 | return np.array(dm.full()) 52 | 53 | 54 | # state symbolic variables 55 | x = ca.SX.sym('x') 56 | y = ca.SX.sym('y') 57 | theta = ca.SX.sym('theta') 58 | states = ca.vertcat( 59 | x, 60 | y, 61 | theta 62 | ) 63 | n_states = states.numel() 64 | 65 | # control symbolic variables 66 | V_a = ca.SX.sym('V_a') 67 | V_b = ca.SX.sym('V_b') 68 | V_c = ca.SX.sym('V_c') 69 | V_d = ca.SX.sym('V_d') 70 | controls = ca.vertcat( 71 | V_a, 72 | V_b, 73 | V_c, 74 | V_d 75 | ) 76 | n_controls = controls.numel() 77 | 78 | # matrix containing all states over all time steps +1 (each column is a state vector) 79 | X = ca.SX.sym('X', n_states, N + 1) 80 | 81 | # matrix containing all control actions over all time steps (each column is an action vector) 82 | U = ca.SX.sym('U', n_controls, N) 83 | 84 | # coloumn vector for storing initial state and target state 85 | P = ca.SX.sym('P', n_states + n_states) 86 | 87 | # state weights matrix (Q_X, Q_Y, Q_THETA) 88 | Q = ca.diagcat(Q_x, Q_y, Q_theta) 89 | 90 | # controls weights matrix 91 | R = ca.diagcat(R1, R2, R3, R4) 92 | 93 | # discretization model (e.g. x2 = f(x1, v, t) = x1 + v * dt) 94 | rot_3d_z = ca.vertcat( 95 | ca.horzcat(cos(theta), -sin(theta), 0), 96 | ca.horzcat(sin(theta), cos(theta), 0), 97 | ca.horzcat( 0, 0, 1) 98 | ) 99 | # Mecanum wheel transfer function which can be found here: 100 | # https://www.researchgate.net/publication/334319114_Model_Predictive_Control_for_a_Mecanum-wheeled_robot_in_Dynamical_Environments 101 | J = (wheel_radius/4) * ca.DM([ 102 | [ 1, 1, 1, 1], 103 | [ -1, 1, 1, -1], 104 | [-1/(Lx+Ly), 1/(Lx+Ly), -1/(Lx+Ly), 1/(Lx+Ly)] 105 | ]) 106 | # RHS = states + J @ controls * step_horizon # Euler discretization 107 | RHS = rot_3d_z @ J @ controls 108 | # maps controls from [va, vb, vc, vd].T to [vx, vy, omega].T 109 | f = ca.Function('f', [states, controls], [RHS]) 110 | 111 | 112 | cost_fn = 0 # cost function 113 | g = X[:, 0] - P[:n_states] # constraints in the equation 114 | 115 | 116 | # runge kutta 117 | for k in range(N): 118 | st = X[:, k] 119 | con = U[:, k] 120 | cost_fn = cost_fn \ 121 | + (st - P[n_states:]).T @ Q @ (st - P[n_states:]) \ 122 | + con.T @ R @ con 123 | st_next = X[:, k+1] 124 | k1 = f(st, con) 125 | k2 = f(st + step_horizon/2*k1, con) 126 | k3 = f(st + step_horizon/2*k2, con) 127 | k4 = f(st + step_horizon * k3, con) 128 | st_next_RK4 = st + (step_horizon / 6) * (k1 + 2 * k2 + 2 * k3 + k4) 129 | g = ca.vertcat(g, st_next - st_next_RK4) 130 | 131 | 132 | OPT_variables = ca.vertcat( 133 | X.reshape((-1, 1)), # Example: 3x11 ---> 33x1 where 3=states, 11=N+1 134 | U.reshape((-1, 1)) 135 | ) 136 | nlp_prob = { 137 | 'f': cost_fn, 138 | 'x': OPT_variables, 139 | 'g': g, 140 | 'p': P 141 | } 142 | 143 | opts = { 144 | 'ipopt': { 145 | 'max_iter': 2000, 146 | 'print_level': 0, 147 | 'acceptable_tol': 1e-8, 148 | 'acceptable_obj_change_tol': 1e-6 149 | }, 150 | 'print_time': 0 151 | } 152 | 153 | solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts) 154 | 155 | lbx = ca.DM.zeros((n_states*(N+1) + n_controls*N, 1)) 156 | ubx = ca.DM.zeros((n_states*(N+1) + n_controls*N, 1)) 157 | 158 | lbx[0: n_states*(N+1): n_states] = -ca.inf # X lower bound 159 | lbx[1: n_states*(N+1): n_states] = -ca.inf # Y lower bound 160 | lbx[2: n_states*(N+1): n_states] = -ca.inf # theta lower bound 161 | 162 | ubx[0: n_states*(N+1): n_states] = ca.inf # X upper bound 163 | ubx[1: n_states*(N+1): n_states] = ca.inf # Y upper bound 164 | ubx[2: n_states*(N+1): n_states] = ca.inf # theta upper bound 165 | 166 | lbx[n_states*(N+1):] = v_min # v lower bound for all V 167 | ubx[n_states*(N+1):] = v_max # v upper bound for all V 168 | 169 | 170 | args = { 171 | 'lbg': ca.DM.zeros((n_states*(N+1), 1)), # constraints lower bound 172 | 'ubg': ca.DM.zeros((n_states*(N+1), 1)), # constraints upper bound 173 | 'lbx': lbx, 174 | 'ubx': ubx 175 | } 176 | 177 | t0 = 0 178 | state_init = ca.DM([x_init, y_init, theta_init]) # initial state 179 | state_target = ca.DM([x_target, y_target, theta_target]) # target state 180 | 181 | # xx = DM(state_init) 182 | t = ca.DM(t0) 183 | 184 | u0 = ca.DM.zeros((n_controls, N)) # initial control 185 | X0 = ca.repmat(state_init, 1, N+1) # initial state full 186 | 187 | 188 | mpc_iter = 0 189 | cat_states = DM2Arr(X0) 190 | cat_controls = DM2Arr(u0[:, 0]) 191 | times = np.array([[0]]) 192 | 193 | 194 | ############################################################################### 195 | 196 | if __name__ == '__main__': 197 | main_loop = time() # return time in sec 198 | while (ca.norm_2(state_init - state_target) > 1e-1) and (mpc_iter * step_horizon < sim_time): 199 | t1 = time() 200 | args['p'] = ca.vertcat( 201 | state_init, # current state 202 | state_target # target state 203 | ) 204 | # optimization variable current state 205 | args['x0'] = ca.vertcat( 206 | ca.reshape(X0, n_states*(N+1), 1), 207 | ca.reshape(u0, n_controls*N, 1) 208 | ) 209 | 210 | sol = solver( 211 | x0=args['x0'], 212 | lbx=args['lbx'], 213 | ubx=args['ubx'], 214 | lbg=args['lbg'], 215 | ubg=args['ubg'], 216 | p=args['p'] 217 | ) 218 | 219 | u = ca.reshape(sol['x'][n_states * (N + 1):], n_controls, N) 220 | X0 = ca.reshape(sol['x'][: n_states * (N+1)], n_states, N+1) 221 | 222 | cat_states = np.dstack(( 223 | cat_states, 224 | DM2Arr(X0) 225 | )) 226 | 227 | cat_controls = np.vstack(( 228 | cat_controls, 229 | DM2Arr(u[:, 0]) 230 | )) 231 | t = np.vstack(( 232 | t, 233 | t0 234 | )) 235 | 236 | t0, state_init, u0 = shift_timestep(step_horizon, t0, state_init, u, f) 237 | 238 | # print(X0) 239 | X0 = ca.horzcat( 240 | X0[:, 1:], 241 | ca.reshape(X0[:, -1], -1, 1) 242 | ) 243 | 244 | # xx ... 245 | t2 = time() 246 | print(mpc_iter) 247 | print(t2-t1) 248 | times = np.vstack(( 249 | times, 250 | t2-t1 251 | )) 252 | 253 | mpc_iter = mpc_iter + 1 254 | 255 | main_loop_time = time() 256 | ss_error = ca.norm_2(state_init - state_target) 257 | 258 | print('\n\n') 259 | print('Total time: ', main_loop_time - main_loop) 260 | print('avg iteration time: ', np.array(times).mean() * 1000, 'ms') 261 | print('final error: ', ss_error) 262 | 263 | # simulate 264 | simulate(cat_states, cat_controls, times, step_horizon, N, 265 | np.array([x_init, y_init, theta_init, x_target, y_target, theta_target]), save=False) 266 | -------------------------------------------------------------------------------- /workshop_github/Python_Implementation/readme.md: -------------------------------------------------------------------------------- 1 | ## Description 2 | This is a Python implementation for model predictive control (MPC) ported from the MATLAB code provided. 3 | 4 | 5 | The implementation uses the Casadi Python Package for numerical optimization and {numpy + matplotlib} for visualization. 6 | A holonomic mobile robot (Mecanum wheeled omnnidirectional mobile robot (MWOMR)) is used as a system for the implementation. 7 | A helpful paper explaining the transfer function: [link from researchgate.net](https://www.researchgate.net/publication/334319114_Model_Predictive_Control_for_a_Mecanum-wheeled_robot_in_Dynamical_Environments) 8 | 9 | 10 | Casadi can be downloaded here https://web.casadi.org/ 11 | 12 | 13 | ## Content 14 | 1. `mpc_code.py` → The main Python script for MPC 15 | 2. `simulation_code.py` → A helper file implementing the visualization used in MPC code 16 | 17 | ## Requirements 18 | Before running the codes, make sure that you have Python3.5+ installed on your computer alongside the following packages: 19 | - [CasADi](https://web.casadi.org/get/) 20 | - [NumPy](https://numpy.org/install/) 21 | - [Matplotlib](https://matplotlib.org/users/installing.html) 22 | 23 | ## Contributors 24 | - Dr. Mohamed Mehrez (Origianl author/owner)
25 | m.mehrez.said@gmail.com
26 | [github.com/MMehrez](github.com/MMehrez)
27 | - Mohammad Abdussalam
28 | mohammadahmad01722@gmail.com
29 | [github.com/Mohammad1722](github.com/Mohammad1722) 30 | - Yassmin Hesham
31 | yasmin98hesham@gmail.com
32 | [github.com/Yasmin-Hesham](github.com/Yasmin-Hesham) 33 | -------------------------------------------------------------------------------- /workshop_github/Python_Implementation/simulation_code.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy import sin, cos, pi 3 | import matplotlib.pyplot as plt 4 | from matplotlib import animation 5 | from time import time 6 | 7 | 8 | def simulate(cat_states, cat_controls, t, step_horizon, N, reference, save=False): 9 | def create_triangle(state=[0,0,0], h=1, w=0.5, update=False): 10 | x, y, th = state 11 | triangle = np.array([ 12 | [h, 0 ], 13 | [0, w/2], 14 | [0, -w/2], 15 | [h, 0 ] 16 | ]).T 17 | rotation_matrix = np.array([ 18 | [cos(th), -sin(th)], 19 | [sin(th), cos(th)] 20 | ]) 21 | 22 | coords = np.array([[x, y]]) + (rotation_matrix @ triangle).T 23 | if update == True: 24 | return coords 25 | else: 26 | return coords[:3, :] 27 | 28 | def init(): 29 | return path, horizon, current_state, target_state, 30 | 31 | def animate(i): 32 | # get variables 33 | x = cat_states[0, 0, i] 34 | y = cat_states[1, 0, i] 35 | th = cat_states[2, 0, i] 36 | 37 | # update path 38 | if i == 0: 39 | path.set_data(np.array([]), np.array([])) 40 | x_new = np.hstack((path.get_xdata(), x)) 41 | y_new = np.hstack((path.get_ydata(), y)) 42 | path.set_data(x_new, y_new) 43 | 44 | # update horizon 45 | x_new = cat_states[0, :, i] 46 | y_new = cat_states[1, :, i] 47 | horizon.set_data(x_new, y_new) 48 | 49 | # update current_state 50 | current_state.set_xy(create_triangle([x, y, th], update=True)) 51 | 52 | # update target_state 53 | # xy = target_state.get_xy() 54 | # target_state.set_xy(xy) 55 | 56 | return path, horizon, current_state, target_state, 57 | 58 | # create figure and axes 59 | fig, ax = plt.subplots(figsize=(6, 6)) 60 | min_scale = min(reference[0], reference[1], reference[3], reference[4]) - 2 61 | max_scale = max(reference[0], reference[1], reference[3], reference[4]) + 2 62 | ax.set_xlim(left = min_scale, right = max_scale) 63 | ax.set_ylim(bottom = min_scale, top = max_scale) 64 | 65 | # create lines: 66 | # path 67 | path, = ax.plot([], [], 'k', linewidth=2) 68 | # horizon 69 | horizon, = ax.plot([], [], 'x-g', alpha=0.5) 70 | # current_state 71 | current_triangle = create_triangle(reference[:3]) 72 | current_state = ax.fill(current_triangle[:, 0], current_triangle[:, 1], color='r') 73 | current_state = current_state[0] 74 | # target_state 75 | target_triangle = create_triangle(reference[3:]) 76 | target_state = ax.fill(target_triangle[:, 0], target_triangle[:, 1], color='b') 77 | target_state = target_state[0] 78 | 79 | sim = animation.FuncAnimation( 80 | fig=fig, 81 | func=animate, 82 | init_func=init, 83 | frames=len(t), 84 | interval=step_horizon*100, 85 | blit=True, 86 | repeat=True 87 | ) 88 | plt.show() 89 | 90 | if save == True: 91 | sim.save('./animation' + str(time()) +'.gif', writer='ffmpeg', fps=30) 92 | 93 | return 94 | 95 | -------------------------------------------------------------------------------- /workshop_github/ReadMe.txt: -------------------------------------------------------------------------------- 1 | Copyright © 2017 by Mohamed W. Mehrez 2 | All rights reserved. 3 | 4 | This is a workshop on implementing model predictive control (MPC) and moving horizon estimation (MHE) on Matlab. 5 | The implementation is based on the Casadi Package which is used for numerical optimization. 6 | A non-holonomic mobile robot is used as a system for the implementation. 7 | The workshop video recording can be found here 8 | https://www.youtube.com/playlist?list=PLK8squHT_Uzej3UCUHjtOtm5X7pMFSgAL 9 | 10 | Casadi can be downloaded here https://web.casadi.org/ 11 | 12 | personal website: https://sites.google.com/view/mwmehrez 13 | e-mail: m.mehrez.said@gmail.com 14 | 15 | If you use the codes, please cite any of my related articles. 16 | you can find my articles on google scholar here 17 | https://scholar.google.ca/citations?user=aRNeH4QAAAAJ&hl=en 18 | 19 | This folder contains the presentation material and codes used in 20 | the MPC workshop taken place at the faculty of Engineering, University of Waterloo, on the 24th and the 25th of January, 2019. 21 | 22 | The contents of the folder are: 23 | 1- The presentation slides. 24 | 2- MATLAB codes of all the examples shown. 25 | 26 | Before running the codes, make sure that casadi package is properly installed and added to the matlab path. 27 | you can download casadi from here https://web.casadi.org/get/ 28 | 29 | please message me in case you have any questions 30 | -------------------------------------------------------------------------------- /workshop_github/model_simulation_using_RK.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MMehrez/MPC-and-MHE-implementation-in-MATLAB-using-Casadi/8937ba42c932e1935bcf394e0566bcf981bc6d33/workshop_github/model_simulation_using_RK.pdf --------------------------------------------------------------------------------