API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Sep 15th, 2019
117
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 8.59 KB | None | 0 0
raw download clone embed print report
  1. %% Initial setup
  2. clc;
  3. close all;
  4. clear all;
  5. colours = [ 0 0.4470 0.7410
  6. 0.8500 0.3250 0.0980
  7. 0.9290 0.6940 0.1250
  8. 0.4940 0.1840 0.5560
  9. 0.4660 0.6740 0.1880
  10. 0.3010 0.7450 0.9330
  11. 0.6350 0.0780 0.1840];
  12. lw = 1.5;
  13. ms = 8;
  14.  
  15. addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
  16. import casadi.*
  17. %------------------------------------------------------
  18. T = 8; % Time horizon
  19. N = 40; % number of control intervals
  20.  
  21. dt = T/N; % length of 1 control interval [s]
  22.  
  23.  
  24.  
  25. %% Continuous dynamics
  26. addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
  27. import casadi.*
  28. %------------------------------------------------------
  29. p = MX.sym('p',3);
  30. yaw = MX.sym('yaw');
  31. v = MX.sym('v',4);
  32.  
  33. u = MX.sym('u',4);
  34. x = [p; yaw; v];
  35.  
  36. tau = 1;
  37.  
  38. R = MX([cos(yaw) -sin(yaw) 0;
  39. sin(yaw) cos(yaw) 0;
  40. 0 0 1]);
  41.  
  42. xdot = [ R*v(1:3); v(4); -(v+u)/tau];
  43.  
  44. % Continuous time dynamics
  45. f = Function('f', {x, u}, {xdot},{'x','u'},{'xdot'});
  46.  
  47.  
  48. %% Integrator
  49. addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
  50. import casadi.*
  51. %------------------------------------------------------
  52. %The problem we want to solve
  53. prob = struct('x',x,'p',u,'ode',f(x,u));
  54. % Runge Kutta order 4
  55. rk_opts = struct;
  56. rk_opts.t0 = 0;
  57. rk_opts.tf = T/N;
  58. rk_opts.number_of_finite_elements = 1;
  59.  
  60. % Legendre order 4
  61. leg_opts = struct;
  62. leg_opts.collocation_scheme = 'legendre';
  63. leg_opts.t0 = 0;
  64. leg_opts.tf = T/N;
  65. leg_opts.number_of_finite_elements = 1;
  66. leg_opts.interpolation_order = 4;
  67.  
  68. % Reference Runge-Kutta implementation
  69. intg = integrator('intg','rk',prob,rk_opts);
  70. res = intg('x0',x,'p',u);
  71.  
  72. % Discretized (sampling time dt) system dynamics as a CasADi Function
  73. F = Function('F', {x, u}, {res.xf},{'x(0)','u(0)'},{'x(dt)'});
  74.  
  75. %%
  76. S = [0.4, 0, 0; 0, 0.1, 0; 0, 0, 0.01];
  77. alpha = 0.01;
  78. % alpha = 1-0.9973;
  79. z_alpha = norminv(1-alpha);
  80.  
  81. r_box_x = 1;
  82. r_box_y = 0.5;
  83.  
  84. r_obs_x = r_box_x*sqrt(2);
  85. r_obs_y = r_box_y*sqrt(2);
  86.  
  87. E = [1/r_obs_x, 0, 0;
  88. 0 1/r_obs_y, 0;
  89. 0 0 0];
  90.  
  91.  
  92.  
  93. %r_box = 1;
  94.  
  95. xo= 5;
  96. yo= -0.01;
  97. zo= 0;
  98.  
  99. xo2 = 1;
  100. yo2 = 4;
  101.  
  102.  
  103. x0 = zeros(numel(x),1);
  104. x0(1) = 0;
  105. x0(2) = 0;
  106. u0 = zeros(numel(u),1);
  107.  
  108. %Plane
  109. v = [x0(1) - xo, x0(2) - yo, x0(3) - zo]';
  110. n = v/norm(v);
  111.  
  112. ref = repmat([10;0;0],1,N+1); size(ref);
  113. x0 = zeros(numel(x),1);
  114. x0(1) = 0;
  115. x0(2) = 0;
  116. u0 = zeros(numel(u),1);
  117.  
  118.  
  119. z_alpha = norminv(1-alpha/N);
  120. %% Optimal control problem (Zhu)
  121. addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
  122. addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
  123. import casadi.*
  124. %------------------------------------------------------
  125. opti = casadi.Opti();
  126. % Decision variables for states
  127. X = opti.variable(numel(x),N+1);
  128. U = opti.variable(numel(u),N);
  129. % Parameter for initial state
  130. %x0 = opti.parameter(nx);
  131.  
  132. % Aliases for states
  133. px = X(1,:);
  134. py = X(2,:);
  135. pz = X(3,:);
  136. theta = X(4,:);
  137. vf = X(5,:);
  138. vs = X(6,:);
  139. vu = X(7,:);
  140. vh = X(8,:);
  141.  
  142. pos = X(1:3,:);
  143.  
  144. % Gap-closing shooting constraints
  145. for k=1:N
  146. opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
  147. %opti.subject_to( n(1)*(px(k)-xo) + n(2)*(py(k)-yo) > r_obs + z_alpha.*sqrt(transpose(n)*S*n));
  148. opti.subject_to( n(1)*(px(k)-xo)/r_obs_x + n(2)*(py(k)-yo)/r_obs_y > 1 + z_alpha.*sqrt(transpose(n)*E*S*E'*n));
  149. end
  150.  
  151. opti.subject_to(X(:,1)==x0);
  152. %Cost
  153. L=[];
  154. % for i= 1:N
  155. % L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
  156. % end
  157. opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
  158. opti.solver('ipopt')
  159.  
  160. sol = opti.solve();
  161.  
  162. sol1.px = sol.value(px);
  163. sol1.py = sol.value(py);
  164. sol1.pz = sol.value(pz);
  165.  
  166. %% Optimal control problem 2 (Kamel)
  167. addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
  168. addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
  169. import casadi.*
  170. %------------------------------------------------------
  171. opti = casadi.Opti();
  172. % Decision variables for states
  173. X = opti.variable(numel(x),N+1);
  174. U = opti.variable(numel(u),N);
  175. % Parameter for initial state
  176. %x0 = opti.parameter(nx);
  177.  
  178. % Aliases for states
  179. px = X(1,:);
  180. py = X(2,:);
  181. pz = X(3,:);
  182. theta = X(4,:);
  183. vf = X(5,:);
  184. vs = X(6,:);
  185. vu = X(7,:);
  186. vh = X(8,:);
  187.  
  188. pos = X(1:3,:);
  189.  
  190. r_max = max([r_obs_x r_obs_y]);
  191. s_max = max(max(S));
  192.  
  193. % Gap-closing shooting constraints
  194. for k=1:N
  195. opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
  196. opti.subject_to( (px(k) - xo).^2 + (py(k)-yo).^2 > (r_max + 3*s_max).^2);
  197. end
  198.  
  199. opti.subject_to(X(:,1)==x0);
  200. %Cost
  201. L=[];
  202. % for i= 1:N
  203. % L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
  204. % end
  205. opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
  206. opti.solver('ipopt')
  207.  
  208. sol = opti.solve();
  209.  
  210. sol2.px = sol.value(px);
  211. sol2.py = sol.value(py);
  212. sol2.pz = sol.value(pz);
  213. %% Optimal control problem (Vasileios)
  214. z_alpha = norminv(1-alpha/(6*N));
  215. addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
  216. addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
  217. import casadi.*
  218. %------------------------------------------------------
  219. opti = casadi.Opti();
  220. % Decision variables for states
  221. X = opti.variable(numel(x),N+1);
  222. U = opti.variable(numel(u),N);
  223. % Parameter for initial state
  224. %x0 = opti.parameter(nx);
  225.  
  226. % Aliases for states
  227. px = X(1,:);
  228. py = X(2,:);
  229. pz = X(3,:);
  230. theta = X(4,:);
  231. vf = X(5,:);
  232. vs = X(6,:);
  233. vu = X(7,:);
  234. vh = X(8,:);
  235.  
  236. pos = X(1:3,:);
  237.  
  238. % Gap-closing shooting constraints
  239. for k=1:N
  240. opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
  241. opti.subject_to( ((px(k) - xo)/(r_box_x + z_alpha*S(1,1))).^2 + ((py(k)-yo)/(r_box_y+ z_alpha*S(2,2))).^2 > 3);
  242. end
  243.  
  244. opti.subject_to(X(:,1)==x0);
  245. %Cost
  246. L=[];
  247. % for i= 1:N
  248. % L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
  249. % end
  250. opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
  251. opti.solver('ipopt')
  252.  
  253. sol = opti.solve();
  254.  
  255. sol4.px = sol.value(px);
  256. sol4.py = sol.value(py);
  257. sol4.pz = sol.value(pz);
  258.  
  259. %% Optimal control problem (Ours)
  260. z_alpha = norminv(1-alpha/N);
  261. addpath('/home/manuel/manuel_ws/casadi_ws/casadi_prog')
  262. addpath('/home/manuel/manuel_ws/matlab_ws/shapes')
  263. import casadi.*
  264. %------------------------------------------------------
  265. opti = casadi.Opti();
  266. % Decision variables for states
  267. X = opti.variable(numel(x),N+1);
  268. U = opti.variable(numel(u),N);
  269. % Parameter for initial state
  270. %x0 = opti.parameter(nx);
  271.  
  272. % Aliases for states
  273. px = X(1,:);
  274. py = X(2,:);
  275. pz = X(3,:);
  276. theta = X(4,:);
  277. vf = X(5,:);
  278. vs = X(6,:);
  279. vu = X(7,:);
  280. vh = X(8,:);
  281.  
  282. pos = X(1:3,:);
  283.  
  284. % Gap-closing shooting constraints
  285. for k=1:N
  286. opti.subject_to(X(:,k+1)==F(X(:,k),U(:,k)));
  287. opti.subject_to( ((px(k) - xo)/(r_box_x + z_alpha*S(1,1))).^2 + ((py(k)-yo)/(r_box_y+ z_alpha*S(2,2))).^2 > 3);
  288. end
  289.  
  290. opti.subject_to(X(:,1)==x0);
  291. %Cost
  292. L=[];
  293. % for i= 1:N
  294. % L = L+(pos(:,i) - ref(:,i)).^2 + U(:,i).^2
  295. % end
  296. opti.minimize(sumsqr(pos-ref) + 2*sumsqr(U));
  297. opti.solver('ipopt')
  298.  
  299. sol = opti.solve();
  300.  
  301. sol3.px = sol.value(px);
  302. sol3.py = sol.value(py);
  303. sol3.pz = sol.value(pz);
  304.  
  305. %% Plot 2D
  306. addpath('/home/manuel/manuel_ws/matlab_ws/plots')
  307. y = linspace(-3,3,100);
  308. x = xo + r_obs_x*(1 + z_alpha.*sqrt(transpose(n)*E*S*E'*n) - n(2)*(y-yo)/r_obs_y)/n(1);
  309. close all;
  310. fig =figure;
  311. hold on
  312. plot(sol1.px,sol1.py,'-o','Color',colours(5,:), 'LineWidth', lw, 'MarkerSize', ms)
  313. plot(sol2.px,sol2.py,'-*','Color',colours(3,:), 'LineWidth', lw, 'MarkerSize', ms)
  314. plot(sol3.px,sol3.py,'->','Color',colours(1,:), 'LineWidth', lw, 'MarkerSize', ms)
  315. %plot(sol4.px,sol4.py,'->','Color',colours(4,:), 'LineWidth', lw, 'MarkerSize', ms)
  316.  
  317. %plot(x,y,'Color',colours(3,:), 'LineWidth', lw, 'MarkerSize', ms)
  318.  
  319. rectangle('Position',[xo-r_obs_x yo-r_obs_y 2*r_obs_x 2*r_obs_y],'Curvature',[1 1],'LineWidth', lw, 'FaceColor', [colours(2,:) 0.25], 'EdgeColor', [colours(2,:) 0.85]);
  320. rectangle('Position',[xo-r_box_x yo-r_box_y 2*r_box_x 2*r_box_y], 'LineWidth', lw, 'FaceColor', [colours(2,:) 0.9], 'EdgeColor', [colours(2,:) 0.85]);
  321. %rectangle('Position',[xo-r_obs yo-r_obs 2*r_obs 2*r_obs],'Curvature',[1 1])
  322. %axis([-2 2 0 6])
  323. %rectangle('Position',[xo-0.2 yo-0.2 0.4 0.4],'Curvature',[0.2 0.2], 'LineWidth', lw, 'FaceColor', 'w', 'EdgeColor', 'none');
  324. %text('Position',[xo-0.05, yo],'string','O','FontSize',15)
  325. ylim([-3 3])
  326. xlim([0 10])
  327. daspect([1 1 1]);
  328. legend('Linearized chance constraint (Zhu et. al. [6])', 'Robust constraint (Kamel et. al. [5])','Our approach','Location','southeast');
  329. xlabel('$x (m)$')
  330. ylabel('$y (m)$')
  331. grid on
  332. width = 6;
  333. height = 5;
  334. fontsize = 14;
  335. printPaperFig(fig, width, height, fontsize,'trajectory');
  336.  
  337. %% Solution
  338. zhu.obj = 3.2140000085882039e+03;
  339. zhu.sec = 0.225;
  340. kamel.obj = 1.3419447587855689e+03;
  341. kamel.sec = 0.652;
  342. vasi.obj = 1.2474557178331086e+03;
  343. vasi.sec = 0.498;
  344. our.obj = 1.2283172186935908e+03;
  345. our.sec = 0.494;
  346. % Relative values
  347. zhu.obj = zhu.obj/our.obj;
  348. zhu.sec = zhu.sec/our.sec
  349. kamel.obj = kamel.obj/our.obj;
  350. kamel.sec = kamel.sec/our.sec
  351. vasi.obj = vasi.obj/our.obj;
  352. vasi.sec = vasi.sec/our.sec
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!