diff --git a/OptimalControl/Drag and Lift/DragAndLift.m b/OptimalControl/Drag and Lift/DragAndLift.m new file mode 100644 index 0000000000..e8c151d2ba --- /dev/null +++ b/OptimalControl/Drag and Lift/DragAndLift.m @@ -0,0 +1,210 @@ +% Launch with drag and lift + +clc; clear; close all + +%% Physical parameters + +g = 9.81; +m = 5.0; +rho = 1.225; +S = 0.6; +Cd0 = 0.03; +k = 0.05; +Cl0 = 0.0; +Clalpha = 0.1*360/(2*pi); + +%% Discretization + +N = 50; % Number of nodes + +%% Initial state + +x0 = 0; y0 = 0; v0 = 15; gamma0 = deg2rad(30); % State initial conditions +alpha0 = deg2rad(5); % Initial guess for angle of attack [rad] +tf0 = 7; % Initial guess for final time [s] + +%% Decision variables vector + +z0 = repmat([x0; y0; v0; gamma0; alpha0], N, 1); +z0 = [z0; tf0]; + +%% Bounds for the variables + +alpha_min = deg2rad(-10); +alpha_max = deg2rad(10); +tf_min = 2; +tf_max = 30; + +lb = repmat([-Inf; 0; 0; -pi/2; alpha_min], N, 1); % Lower bounds +ub = repmat([ Inf; Inf; Inf; pi/2; alpha_max], N, 1); % Upper bounds +lb = [lb; tf_min]; +ub = [ub; tf_max]; + +%% Cost function: maximize range + +obj = @(z) -z((N-1)*5 + 1); % Cost function x(t_f) + +%% Restrictions + +nonlcon = @(z) collocation_constraints_tf(z, N, rho, S, Cd0, k, Cl0, Clalpha, m, g, v0, gamma0); + +%% Optimization + +opts = optimoptions('fmincon', ... + 'Display','iter-detailed', ... + 'MaxFunctionEvaluations', 1e6, ... + 'MaxIterations', 1000, ... + 'StepTolerance', 1e-6, ... + 'ScaleProblem', true, ... + 'Algorithm', 'interior-point'); + +[z_opt, fval] = fmincon(obj, z0, [], [], [], [], lb, ub, nonlcon, opts); + +%% Final results + +tf_opt = z_opt(end); % Optimal final time [s] +Z = reshape(z_opt(1:end-1), 5, N); +x = Z(1,:); y = Z(2,:); v = Z(3,:); gamma = Z(4,:); alpha = Z(5,:); +t = linspace(0, tf_opt, N); + +q = 0.5*rho.*v.^2; +Cl = Cl0+Clalpha.*alpha; +Cd = Cd0+k.*Cl.^2; +L = q.*S.*Cl; +D = q.*S.*Cd; + +fprintf('Máximum range: %.2f m\n', x(end)); +fprintf('Optimal final time: %.2f s\n', tf_opt); + +%% Postprocess + +set(groot,'defaultLineLineWidth',1.5); % thicker lines for all plots + +% Optimal trajectory +figure +plot(x, y, 'LineWidth', 2,'DisplayName','Trajectory') +hold on +plot(x(1), y(1), 'go', 'MarkerSize', 7, 'MarkerFaceColor', 'g', 'DisplayName','Launch'); +plot(x(end),y(end),'ro', 'MarkerSize', 7, 'MarkerFaceColor','r','DisplayName','Impact'); +xlim([0 1.05*max(x)]) +ylim([min(0,1.05*min(y)) 1.05*max(y)]) +grid on; box on +xlabel('x [m]','Interpreter','latex') +ylabel('y [m]','Interpreter','latex') +title('Optimal trajectory','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) + +% Angle-of-attack profile +figure +stairs(t, rad2deg(alpha), 'Marker','o') +grid on; box on +xlim([0 t(end)]) +ylim([min(rad2deg(alpha))-2 max(rad2deg(alpha))+2]) +xlabel('Time [s]','Interpreter','latex') +ylabel('$\alpha$ [$^\circ$]','Interpreter','latex') +title('Angle of attack profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Speed vs. time +figure +plot(t, v, 'LineWidth', 2) +grid on; +xlabel('Time [s]','Interpreter','latex') +ylabel('$v$ [m/s]','Interpreter','latex') +title('Velocity profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Flight-path angle vs. time +figure +plot(t, rad2deg(gamma), 'LineWidth', 2) +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('$\gamma$ [$^\circ$]','Interpreter','latex') +title('Flight path angle','Interpreter','latex') +set(gca,'FontSize',12) + +% Lift & Drag vs. time + +figure +plot(t, L,'DisplayName','Lift','LineWidth',2); +hold on +plot(t, D, 'DisplayName','Drag','LineWidth',2); +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('Force [N]','Interpreter','latex') +title('Aerodynamic forces','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) + +%% ---------- FUNCTIONS ---------- + +function [c, ceq] = collocation_constraints_tf(z, N, rho, S, Cd0, k, Cl0, Clalpha, m, g, v0, gamma0) + tf = z(end); + dt = tf / (N - 1); + Z = reshape(z(1:end-1), 5, N); + x = Z(1,:); y = Z(2,:); v = Z(3,:); gamma = Z(4,:); alpha = Z(5,:); + + ceq = []; + for i = 1:N-1 + % States at i and i+1 + xi = x(i); yi = y(i); vi = v(i); gi = gamma(i); ai = alpha(i); + xj = x(i+1); yj = y(i+1); vj = v(i+1); gj = gamma(i+1); aj = alpha(i+1); + + % Derivatives + [dxi, dyi, dvi, dgi] = dynamics(xi, yi, vi, gi, ai, rho, S, Cd0, k, Cl0, Clalpha, m, g); + [dxj, dyj, dvj, dgj] = dynamics(xj, yj, vj, gj, aj, rho, S, Cd0, k, Cl0, Clalpha, m, g); + + % Collocation equations (trapezoidal) + ceq = [ceq; + xj-xi-dt/2*(dxi + dxj); + yj-yi-dt/2*(dyi + dyj); + vj-vi-dt/2*(dvi + dvj); + gj-gi-dt/2*(dgi + dgj)]; + end + + % Initial conditions + ceq = [ceq; + x(1)-0; + y(1)-0; + v(1)-v0; + gamma(1)-gamma0]; + + % Equality constraint: y(tf) = 0 + ceq = [ceq; + y(end)]; + + % Inequality constraint: y_i ≥ 0 + c = -y; + + % Optimal trajectory +figure(100) +plot(x, y, 'LineWidth', 2,'DisplayName','Trajectory') +hold on +plot(x(1), y(1), 'go', 'MarkerSize', 7, 'MarkerFaceColor', 'g', 'DisplayName','Launch'); +plot(x(end),y(end),'ro', 'MarkerSize', 7, 'MarkerFaceColor','r','DisplayName','Impact'); +xlim([0 1.05*max(x)+0.01]) +ylim([min(0,1.05*min(y)) 1.05*max(y)]) +grid on; box on +xlabel('x [m]','Interpreter','latex') +ylabel('y [m]','Interpreter','latex') +title('Optimal trajectory','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) +hold off + +end + +function [dx, dy, dv, dgamma] = dynamics(x, y, v, gamma, alpha, rho, S, Cd0, k, Cl0, Clalpha, m, g) + Cl = Cl0+Clalpha*alpha; + Cd = Cd0+k* Cl^2; + q = 0.5*rho*v^2; + L = q*S*Cl; + D = q*S*Cd; + + dx = v*cos(gamma); + dy = v*sin(gamma); + dv = -D/m-g*sin(gamma); + dgamma = L/(m*v)-g*cos(gamma)/v; + +end diff --git a/OptimalControl/Drag and Lift/TrapezoidalDynamics.m b/OptimalControl/Drag and Lift/TrapezoidalDynamics.m new file mode 100644 index 0000000000..8345ed455d --- /dev/null +++ b/OptimalControl/Drag and Lift/TrapezoidalDynamics.m @@ -0,0 +1,197 @@ +clc; clear; close all + +%% Physical parameters + +g = 9.81; % gravity [m/s^2] +rho = 1.225; % air density [kg/m^3] +Sw = 0.6; % aerodynamic surface [m^2] +Cd0 = 0.03; +m = 5; % mass [kg] +k = 0.05; +Clalpha = 0.1*360/(2*pi); +Cl0 = 0; + +%% Discretization + +N = 250; % number of nodes + +%% Initial state + +x1_0 = 0; x2_0 = 0; v0 = 15; gamma0 = deg2rad(30); +t0 = 0; % Initial time [s] +tf0 = 7; % Initial guess for final time [s] +alpha0 = deg2rad(5); % Initial guess for the angle of attack [rad] + +%% Control vector + +u0 = [tf0 alpha0*ones(1,N)]; + +%% Bounds for the control + +alpha_min = deg2rad(-10); +alpha_max = deg2rad(10); +tf_min = 0.1; +tf_max = 15; + +lb = [tf_min alpha_min*ones(1,N)]; % lower bound +ub = [tf_max alpha_max*ones(1,N)]; % upper bound + +%% Cost function: maximize range + +cost = @(u) f_cost(u, v0, gamma0, x1_0, x2_0, g, Cl0, Clalpha, Cd0, k, Sw, m, N, rho); + +%% Restrictions + +nonlcon_fun = @(u) nonlcon(u, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m, N, rho); + +%% Optimization solver + +options = optimoptions('fmincon','Algorithm', 'interior-point', ... + 'TolCon', 1e-8,'Display', 'iter','MaxFunctionEvaluations',1e6, ... + 'ScaleProblem',true,'StepTolerance',1e-6,'MaxIter',2000); + +[u_opt, ~] = fmincon(cost, u0, [], [], [], [], lb, ub, nonlcon_fun, options); + +%% Final results + +tf = u_opt(1); +alpha = u_opt(2:N+1); +t_span = linspace(t0, tf, N); +dt = tf / (N-1); + +z = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m, rho); % State vector solution +x = z(1,:); % Horizontal displacement [m] +y = z(2,:); % Vertical displacement [m] +v = z(3,:); % Velocity [m/s] +gamma = z(4,:); % Flight path angle [°] + +q = 0.5*rho.*v.^2; +Cl = Cl0+Clalpha.*alpha; +Cd = Cd0+k.*Cl.^2; +L = q.*Sw.*Cl; +D = q.*Sw.*Cd; + +disp(["Maximum distance [m] = ", num2str(x(end))]) +disp(["Final Time [s]: ", num2str(tf)]) + +[c_val, ceq_val] = nonlcon(u_opt, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m, N, rho); +max_ineq = max(c_val); +final_eq = ceq_val; + +disp(['Max. violation of inequality constraint: ', num2str(max_ineq)]) +disp(['Violation of equality constraint y(tf)=0: ', num2str(final_eq)]) + +validateGradient(u0, v0, gamma0, x1_0, x2_0, g, Cl0, Clalpha, Cd0, k, Sw, m, N, rho) + +%% Postprocess + +% Optimal trajectory +figure +plot(x, y, 'LineWidth', 2,'DisplayName','Trajectory') +hold on +plot(x(1), y(1), 'go', 'MarkerSize', 7, 'MarkerFaceColor', 'g', 'DisplayName','Launch'); +plot(x(end),y(end),'ro', 'MarkerSize', 7, 'MarkerFaceColor','r','DisplayName','Impact'); +xlim([0 1.05*max(x)]) +ylim([min(0,1.05*min(y)) 1.05*max(y)]) +grid on; box on +xlabel('x [m]','Interpreter','latex') +ylabel('y [m]','Interpreter','latex') +title('Optimal trajectory','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) + +% Angle-of-attack profile +figure +stairs(t_span, rad2deg(alpha), 'Marker','o') +grid on; box on +xlim([0 t_span(end-1)]) +ylim([min(rad2deg(alpha))-2 max(rad2deg(alpha))+2]) +xlabel('Time [s]','Interpreter','latex') +ylabel('$\alpha$ [$^\circ$]','Interpreter','latex') +title('Angle of attack profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Speed vs. time +figure +plot(t_span, v, 'LineWidth', 2) +grid on; +xlabel('Time [s]','Interpreter','latex') +ylabel('$v$ [m/s]','Interpreter','latex') +title('Velocity profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Flight-path angle vs. time +figure +plot(t_span, rad2deg(gamma), 'LineWidth', 2) +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('$\gamma$ [$^\circ$]','Interpreter','latex') +title('Flight path angle','Interpreter','latex') +set(gca,'FontSize',12) + +% Lift & Drag vs. time + +figure +plot(t_span(1:end-1), L(1:end-1),'DisplayName','Lift','LineWidth',2); +hold on +plot(t_span(1:end-1), D(1:end-1), 'DisplayName','Drag','LineWidth',2); +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('Force [N]','Interpreter','latex') +title('Aerodynamic forces','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) + +%% ---------- FUNCTIONS ---------- + +function [J] = f_cost(u, v0, gamma0, x1_0, x2_0, g, Cl0, Clalpha, Cd0, k, Sw, m, N, rho) + tf = u(1); + alpha = u(2:N+1); + + y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m, rho); + J = -y(1,end); % maximize range +end + +function [c, ceq] = nonlcon(u, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m, N, rho) + tf = u(1); + alpha = u(2:N+1); + y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m, rho); + c = -y(2,:); % y >= 0 + ceq = y(2,end); % y(tf) = 0 +end + +function y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m, rho) + t_span = linspace(0, tf, N); + dt = tf / (N-1); + y = zeros(4,N); + y(:,1) = [x1_0; x2_0; v0; gamma0]; + for i = 1:N-1 + ti = t_span(i); + alphai = alpha(i); + alphai1 = alpha(i+1); + yi = y(:,i); + + fi = dynamics_vector(ti, yi, alphai, g, Cl0, Clalpha, Cd0, k, Sw, m, rho); + y_pred = yi + dt*fi; + fi1 = dynamics_vector(t_span(i+1), y_pred, alphai1, g, Cl0, Clalpha, Cd0, k, Sw, m, rho); + y(:,i+1) = yi + (dt/2)*(fi + fi1); + end +end + +function dydt = dynamics_vector(~, y, alpha, g, Cl0, Clalpha, Cd0, k, Sw, m, rho) + v = y(3); + gamma = y(4); + + Cl = Cl0+Clalpha*alpha; + Cd = Cd0+k*Cl^2; + q = 0.5*rho*v^2; + D = q*Sw*Cd; + L = q*Sw*Cl; + + dydt = [ + v*cos(gamma); + v*sin(gamma); + -g*sin(gamma) - D/m; + -(g/v)*cos(gamma) + L/(m*v) + ]; +end diff --git a/OptimalControl/Thrust/Thrust10.m b/OptimalControl/Thrust/Thrust10.m new file mode 100644 index 0000000000..57772742bf --- /dev/null +++ b/OptimalControl/Thrust/Thrust10.m @@ -0,0 +1,234 @@ +clc; clear; close all + +%% Physical parameters + +g = 9.81; % gravity [m/s^2] +rho = 1.225; % air density [kg/m^3] +Sw = 0.6; % aerodynamic surface [m^2] +mstruct = 4.5; % structural mass [kg] +Cd0 = 0.03; +k = 0.05; +Clalpha = 0.1*360/(2*pi); +Cl0 = 0; + +%% Discretization + +N = 200; % number of nodes +% load("resultsN300.mat") +% u_opt300 = u_opt; clear u_opt +% alpha0 = interp1(linspace(0,u_opt300(1),300),u_opt300(2:301),linspace(0,u_opt300(1),N)); +% T0 = interp1(linspace(0,u_opt300(1),300),u_opt300(302:end),linspace(0,u_opt300(1),N)); + +%% Initial state + +x1_0 = 0; x2_0 = 0; v0 = 15; gamma0 = deg2rad(30); m0 = 5; +t0 = 0; % Initial time [s] +tf0 = 10; % Initial guess for final time [s] +alpha0 = deg2rad(5); % Initial guess for the angle of attack [rad] +T0 = 8; +mfuel = m0 - mstruct; % fuel mass [kg] + +%% Control vector + +u0 = [tf0 alpha0*ones(1,N) T0*ones(1,N)]; + +%% Bounds for the control + +alpha_min = deg2rad(-10); +alpha_max = deg2rad(10); +tf_min = 0.1; +tf_max = 15; +Tmin = 0; +Tmax = 10; + +lb = [tf_min alpha_min*ones(1,N) Tmin*ones(1,N)]; % lower bound +ub = [tf_max alpha_max*ones(1,N) Tmax*ones(1,N)]; % upper bound + +%% Cost function: maximize range + +cost = @(u) f_cost(u, v0, gamma0, x1_0, x2_0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho); + +%% Restrictions + +nonlcon_fun = @(u) nonlcon(u, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho, mstruct); + +%% Optimization solver + +options = optimoptions('fmincon','Algorithm', 'interior-point', ... + 'TolCon', 1e-9,'Display', 'iter','MaxFunctionEvaluations',1e6, ... + 'ScaleProblem',true,'StepTolerance',1e-6,'MaxIter',2000); + +[u_opt, ~] = fmincon(cost, u0, [], [], [], [], lb, ub, nonlcon_fun, options); + +%% Final results + +tf = u_opt(1); +alpha = u_opt(2:N+1); +T = u_opt(N+2:end); +t_span = linspace(t0, tf, N); +dt = tf / (N-1); + +z = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T); % State vector solution +x = z(1,:); % Horizontal displacement [m] +y = z(2,:); % Vertical displacement [m] +v = z(3,:); % Velocity [m/s] +gamma = z(4,:); % Flight path angle [°] +m = z(5,:); + +q = 0.5*rho.*v.^2; +Cl = Cl0+Clalpha.*alpha; +Cd = Cd0+k.*Cl.^2; +L = q.*Sw.*Cl; +D = q.*Sw.*Cd; + +disp(["Maximum distance [m] = ", num2str(x(end))]) +disp(["Final Time [s]: ", num2str(tf)]) + +[c_val, ceq_val] = nonlcon(u_opt, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho, mstruct); +max_ineq = max(c_val); +final_eq = ceq_val; + +%disp(['Max. violation of inequality constraint: ', num2str(max_ineq)]) +%disp(['Violation of equality constraint y(tf)=0: ', num2str(final_eq)]) + +%% Postprocess + +% Optimal trajectory +figure +%plot(x, y, 'LineWidth', 5,'DisplayName','Trajectory','HandleVisibility','off') +hold on +plot(x(1), y(1), 'go', 'MarkerSize', 7, 'MarkerFaceColor', 'g', 'DisplayName','Launch'); +plot(x(end),y(end),'ro', 'MarkerSize', 7, 'MarkerFaceColor','r','DisplayName','Impact'); +xlim([0 1.05*max(x)]) +ylim([min(0,1.05*min(y)) 1.05*max(y)]) +grid on; box on +xlabel('x [m]','Interpreter','latex') +ylabel('y [m]','Interpreter','latex') +title('Optimal trajectory','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) +patch([x NaN], [y NaN], [T NaN],'EdgeColor','interp', 'LineWidth', 4, ... + 'FaceColor','none','HandleVisibility','off'); +colormap("turbo") +c = colorbar; +c.Label.String = 'Thrust [N]'; + +% Angle-of-attack profile +figure +stairs(t_span, rad2deg(alpha), 'Marker','o') +grid on; box on +xlim([0 t_span(end-1)]) +ylim([min(rad2deg(alpha))-2 max(rad2deg(alpha))+2]) +xlabel('Time [s]','Interpreter','latex') +ylabel('$\alpha$ [$^\circ$]','Interpreter','latex') +title('Angle of attack profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Speed vs. time +figure +plot(t_span, v, 'LineWidth', 2) +grid on; +xlabel('Time [s]','Interpreter','latex') +ylabel('$v$ [m/s]','Interpreter','latex') +title('Velocity profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Flight-path angle vs. time +figure +plot(t_span(1:end-1), rad2deg(gamma(1:end-1)), 'LineWidth', 2) +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('$\gamma$ [$^\circ$]','Interpreter','latex') +title('Flight path angle','Interpreter','latex') +set(gca,'FontSize',12) + +% Lift & Drag vs. time + +figure +plot(t_span(1:end-1), L(1:end-1),'DisplayName','Lift','LineWidth',2); +hold on +plot(t_span(1:end-1), D(1:end-1), 'DisplayName','Drag','LineWidth',2); +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('Force [N]','Interpreter','latex') +title('Aerodynamic forces','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) + +% Thrust vs. time + +figure +plot(t_span,T,'DisplayName','Thrust','LineWidth',2) +grid on +xlabel('Time [s]','Interpreter','latex') +ylabel('Thrust [N]','Interpreter','latex') +title('Thrust profile','Interpreter','latex') + +% Mass vs. time + +figure +plot(t_span, m,'LineWidth',2) +grid on +xlabel('Time [s]','Interpreter','latex') +ylabel('Thrust [N]','Interpreter','latex') +title('Mass over time','Interpreter','latex') + +%% ---------- FUNCTIONS ---------- + +function [J] = f_cost(u, v0, gamma0, x1_0, x2_0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho) + tf = u(1); + alpha = u(2:N+1); + T = u(N+2:end); + + y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T); + J = -y(1,end); % maximize range +end + +function [c, ceq] = nonlcon(u, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho, mstruct) + tf = u(1); + alpha = u(2:N+1); + T = u(N+2:end); + y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T); + c = [-y(2,:); mstruct-y(5,:)]; % y >= 0 + ceq = [y(2,end); y(5,end)-mstruct]; % y(tf) = 0 +end + +function y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T) + t_span = linspace(0, tf, N); + dt = tf / (N-1); + y = zeros(5,N); + y(:,1) = [x1_0; x2_0; v0; gamma0; m0]; + for i = 1:N-1 + ti = t_span(i); + alphai = alpha(i); + alphai1 = alpha(i+1); + Ti = T(i); + Ti1 = T(i+1); + yi = y(:,i); + + fi = dynamics_vector(ti, yi, alphai, g, Cl0, Clalpha, Cd0, k, Sw, rho, Ti); + y_pred = yi + dt*fi; + fi1 = dynamics_vector(t_span(i+1), y_pred, alphai1, g, Cl0, Clalpha, Cd0, k, Sw, rho, Ti1); + y(:,i+1) = yi + (dt/2)*(fi + fi1); + end +end + +function dydt = dynamics_vector(~, y, alpha, g, Cl0, Clalpha, Cd0, k, Sw, rho, T) + v = y(3); + gamma = y(4); + m = y(5); + + Cl = Cl0+Clalpha*alpha; + Cd = Cd0+k*Cl^2; + q = 0.5*rho*v^2; + D = q*Sw*Cd; + L = q*Sw*Cl; + + dydt = [ + v*cos(gamma); + v*sin(gamma); + -g*sin(gamma) - D/m + T/m; + -(g/v)*cos(gamma) + L/(m*v); + -T/20; + ]; +end diff --git a/OptimalControl/Thrust/Thrust2_5.m b/OptimalControl/Thrust/Thrust2_5.m new file mode 100644 index 0000000000..da2597b9e5 --- /dev/null +++ b/OptimalControl/Thrust/Thrust2_5.m @@ -0,0 +1,230 @@ +clc; clear; close all + +%% Physical parameters + +g = 9.81; % gravity [m/s^2] +rho = 1.225; % air density [kg/m^3] +Sw = 0.6; % aerodynamic surface [m^2] +mstruct = 4.5; % structural mass [kg] +Cd0 = 0.03; +k = 0.05; +Clalpha = 0.1*360/(2*pi); +Cl0 = 0; + +%% Discretization + +N = 250; % number of nodes + +%% Initial state + +x1_0 = 0; x2_0 = 0; v0 = 15; gamma0 = deg2rad(30); m0 = 5; +t0 = 0; % Initial time [s] +tf0 = 10; % Initial guess for final time [s] +alpha0 = deg2rad(5); % Initial guess for the angle of attack [rad] +T0 = 2; +mfuel = m0 - mstruct; % fuel mass [kg] + +%% Control vector + +u0 = [tf0 alpha0*ones(1,N) T0*ones(1,N)]; + +%% Bounds for the control + +alpha_min = deg2rad(-10); +alpha_max = deg2rad(10); +tf_min = 0.1; +tf_max = 15; +Tmin = 0; +Tmax = 2.5; + +lb = [tf_min alpha_min*ones(1,N) Tmin*ones(1,N)]; % lower bound +ub = [tf_max alpha_max*ones(1,N) Tmax*ones(1,N)]; % upper bound + +%% Cost function: maximize range + +cost = @(u) f_cost(u, v0, gamma0, x1_0, x2_0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho); + +%% Restrictions + +nonlcon_fun = @(u) nonlcon(u, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho, mstruct); + +%% Optimization solver + +options = optimoptions('fmincon','Algorithm', 'interior-point', ... + 'TolCon', 1e-9,'Display', 'iter','MaxFunctionEvaluations',1e6, ... + 'ScaleProblem',true,'StepTolerance',1e-6,'MaxIter',2000); + +[u_opt, ~] = fmincon(cost, u0, [], [], [], [], lb, ub, nonlcon_fun, options); + +%% Final results + +tf = u_opt(1); +alpha = u_opt(2:N+1); +T = u_opt(N+2:end); +t_span = linspace(t0, tf, N); +dt = tf / (N-1); + +z = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T); % State vector solution +x = z(1,:); % Horizontal displacement [m] +y = z(2,:); % Vertical displacement [m] +v = z(3,:); % Velocity [m/s] +gamma = z(4,:); % Flight path angle [°] +m = z(5,:); + +q = 0.5*rho.*v.^2; +Cl = Cl0+Clalpha.*alpha; +Cd = Cd0+k.*Cl.^2; +L = q.*Sw.*Cl; +D = q.*Sw.*Cd; + +disp(["Maximum distance [m] = ", num2str(x(end))]) +disp(["Final Time [s]: ", num2str(tf)]) + +[c_val, ceq_val] = nonlcon(u_opt, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho, mstruct); +max_ineq = max(c_val); +final_eq = ceq_val; + +%disp(['Max. violation of inequality constraint: ', num2str(max_ineq)]) +%disp(['Violation of equality constraint y(tf)=0: ', num2str(final_eq)]) + +%% Postprocess + +% Optimal trajectory +figure +%plot(x, y, 'LineWidth', 5,'DisplayName','Trajectory','HandleVisibility','off') +hold on +plot(x(1), y(1), 'go', 'MarkerSize', 7, 'MarkerFaceColor', 'g', 'DisplayName','Launch'); +plot(x(end),y(end),'ro', 'MarkerSize', 7, 'MarkerFaceColor','r','DisplayName','Impact'); +xlim([0 1.05*max(x)]) +ylim([min(0,1.05*min(y)) 1.05*max(y)]) +grid on; box on +xlabel('x [m]','Interpreter','latex') +ylabel('y [m]','Interpreter','latex') +title('Optimal trajectory','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) +patch([x NaN], [y NaN], [T NaN],'EdgeColor','interp', 'LineWidth', 4, ... + 'FaceColor','none','HandleVisibility','off'); +colormap("turbo") +c = colorbar; +c.Label.String = 'Thrust [N]'; + +% Angle-of-attack profile +figure +stairs(t_span, rad2deg(alpha), 'Marker','o') +grid on; box on +xlim([0 t_span(end-1)]) +ylim([min(rad2deg(alpha))-2 max(rad2deg(alpha))+2]) +xlabel('Time [s]','Interpreter','latex') +ylabel('$\alpha$ [$^\circ$]','Interpreter','latex') +title('Angle of attack profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Speed vs. time +figure +plot(t_span, v, 'LineWidth', 2) +grid on; +xlabel('Time [s]','Interpreter','latex') +ylabel('$v$ [m/s]','Interpreter','latex') +title('Velocity profile','Interpreter','latex') +set(gca,'FontSize',12) + +% Flight-path angle vs. time +figure +plot(t_span(1:end-1), rad2deg(gamma(1:end-1)), 'LineWidth', 2) +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('$\gamma$ [$^\circ$]','Interpreter','latex') +title('Flight path angle','Interpreter','latex') +set(gca,'FontSize',12) + +% Lift & Drag vs. time + +figure +plot(t_span(1:end-1), L(1:end-1),'DisplayName','Lift','LineWidth',2); +hold on +plot(t_span(1:end-1), D(1:end-1), 'DisplayName','Drag','LineWidth',2); +grid on; box on +xlabel('Time [s]','Interpreter','latex') +ylabel('Force [N]','Interpreter','latex') +title('Aerodynamic forces','Interpreter','latex') +legend('Location','best') +set(gca,'FontSize',12) + +% Thrust vs. time + +figure +plot(t_span,T,'DisplayName','Thrust','LineWidth',2) +grid on +xlabel('Time [s]','Interpreter','latex') +ylabel('Thrust [N]','Interpreter','latex') +title('Thrust profile','Interpreter','latex') + +% Mass vs. time + +figure +plot(t_span, m,'LineWidth',2) +grid on +xlabel('Time [s]','Interpreter','latex') +ylabel('Thrust [N]','Interpreter','latex') +title('Mass over time','Interpreter','latex') + +%% ---------- FUNCTIONS ---------- + +function [J] = f_cost(u, v0, gamma0, x1_0, x2_0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho) + tf = u(1); + alpha = u(2:N+1); + T = u(N+2:end); + + y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T); + J = -y(1,end); % maximize range +end + +function [c, ceq] = nonlcon(u, v0, x1_0, x2_0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, N, m0, rho, mstruct) + tf = u(1); + alpha = u(2:N+1); + T = u(N+2:end); + y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T); + c = [-y(2,:); mstruct-y(5,:)]; % y >= 0 + ceq = [y(2,end); y(5,end)-mstruct]; % y(tf) = 0 +end + +function y = trapezoidal(tf, alpha, N, x1_0, x2_0, v0, gamma0, g, Cl0, Clalpha, Cd0, k, Sw, m0, rho, T) + t_span = linspace(0, tf, N); + dt = tf / (N-1); + y = zeros(5,N); + y(:,1) = [x1_0; x2_0; v0; gamma0; m0]; + for i = 1:N-1 + ti = t_span(i); + alphai = alpha(i); + alphai1 = alpha(i+1); + Ti = T(i); + Ti1 = T(i+1); + yi = y(:,i); + + fi = dynamics_vector(ti, yi, alphai, g, Cl0, Clalpha, Cd0, k, Sw, rho, Ti); + y_pred = yi + dt*fi; + fi1 = dynamics_vector(t_span(i+1), y_pred, alphai1, g, Cl0, Clalpha, Cd0, k, Sw, rho, Ti1); + y(:,i+1) = yi + (dt/2)*(fi + fi1); + end +end + +function dydt = dynamics_vector(~, y, alpha, g, Cl0, Clalpha, Cd0, k, Sw, rho, T) + v = y(3); + gamma = y(4); + m = y(5); + + Cl = Cl0+Clalpha*alpha; + Cd = Cd0+k*Cl^2; + q = 0.5*rho*v^2; + D = q*Sw*Cd; + L = q*Sw*Cl; + + dydt = [ + v*cos(gamma); + v*sin(gamma); + -g*sin(gamma) - D/m + T/m; + -(g/v)*cos(gamma) + L/(m*v); + -T/20; + ]; +end diff --git a/OptimalControl/old/cesc/Shooting.m b/OptimalControl/old/cesc/Shooting.m new file mode 100644 index 0000000000..ab5391fd15 --- /dev/null +++ b/OptimalControl/old/cesc/Shooting.m @@ -0,0 +1,131 @@ +% INDIRECT METHOD => Shooting technique to solve boundary problem +% Solver for root finding -> fsolve + + +clc; +clear; + +global error_x_history error_y_history +error_x_history = []; +error_y_history = []; + +% Initial guess for [k, tf] + +guess = [0, 40]; % k is c3 in the notes. tf is the final time + +% Root-finding (using fsolve) to compute optimal values of k and tf + +options = optimoptions('fsolve', 'Display', 'iter', 'FunctionTolerance', 1e-6); + +solution = fsolve(@residual_function, guess, options); + +k_sol = solution(1); +tf_sol = solution(2); + +fprintf('\nSolution found:\n'); +fprintf('k = %.6f\n', k_sol); +fprintf('tf = %.6f\n', tf_sol); + +% Integrate final trajectory once optimal k and tf are obtained + +[x_sol, y_sol, t_sol] = integrate_trajectory(k_sol, tf_sol); + +% Results plot + +set(groot, 'defaultTextInterpreter', 'latex'); +set(groot, 'defaultAxesTickLabelInterpreter', 'latex'); +set(groot, 'defaultLegendInterpreter', 'latex'); + +figure; +plot(x_sol, y_sol, 'b-', 'LineWidth', 2); +hold on; +plot(0, 0, 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % Start +plot(1000, 200, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % Target +grid on; +xlabel('x'); +ylabel('y'); +title('Optimal Trajectory Found Using Shooting Method'); +legend('Trajectory', 'Start', 'Target'); +ylim([0 250]) +xlim([0 1000]) + +t_plot = linspace(0, tf_sol, 1000); +alpha_plot = atan(-0.5 * t_plot + k_sol); + +figure; +plot(t_plot, rad2deg(alpha_plot), 'r', 'LineWidth', 2); +grid on; +xlabel('Time [s]'); +ylabel('$\alpha$ [deg]'); +title('Control Angle $\alpha$ Over Time'); + +V = 30; +xdot = V * cos(alpha_plot); +ydot = V * sin(alpha_plot); + +figure; +plot(t_plot, xdot, 'b', 'LineWidth', 2); +hold on; +plot(t_plot, ydot, 'm--', 'LineWidth', 2); +grid on; +xlabel('Time [s]'); +ylabel('Velocity Components [m/s]'); +legend('$v_x$', '$v_y$'); +title('Velocity Components Over Time'); + +figure; +subplot(2,1,1); +plot(1:length(error_x_history), error_x_history, 'b-o', 'LineWidth', 2); +grid on; +xlabel('Iteration'); +ylabel('Error in x [m]'); +title('Error in $x(t_f)$ Over FSOLVE Iterations'); + +subplot(2,1,2); +plot(1:length(error_y_history), error_y_history, 'r-s', 'LineWidth', 2); +grid on; +xlabel('Iteration'); +ylabel('Error in y [m]'); +title('Error in $y(t_f)$ Over FSOLVE Iterations'); + + +% Function Definitions + +function F = residual_function(params) + global error_x_history error_y_history + k = params(1); + tf = params(2); + + % Integrate system from t=0 to t=tf + [x_sol, y_sol, ~] = integrate_trajectory(k, tf); + + x_tf = x_sol(end); + y_tf = y_sol(end); + + % Errores con respecto al objetivo + ex = x_tf - 1000; + ey = y_tf - 200; + + % Guardar los errores por separado + error_x_history(end+1) = ex; + error_y_history(end+1) = ey; + + % Boundary errors + F = [x_tf - 1000; y_tf - 200]; +end + +function [x_vals, y_vals, t_vals] = integrate_trajectory(k, tf) + V = 30; + z0 = [0; 0]; % Initial conditions: x(0)=0, y(0)=0 + + % Define ODE system + odefun = @(t, z) [ + V * cos(atan(-0.5 * t + k)) + 0.5 * z(2); % xdot + V * sin(atan(-0.5 * t + k)) % ydot + ]; + + % Integrate with ode45 + [t_vals, z] = ode45(odefun, [0 tf], z0); + x_vals = z(:,1); + y_vals = z(:,2); +end diff --git a/OptimalControl/old/cesc/Trapezoidal_direct.m b/OptimalControl/old/cesc/Trapezoidal_direct.m new file mode 100644 index 0000000000..2669448c48 --- /dev/null +++ b/OptimalControl/old/cesc/Trapezoidal_direct.m @@ -0,0 +1,119 @@ +% DIRECT COLLOCATION METHOD => Solved with own transcription +% Trapezoidal discretization +% NLP solver -> fmincon + +clear; +clc; + +% Problem inputs + +N = 50; % number of mesh intervals +x0 = 0; y0 = 0; % initial state +xf = 1000; yf = 200; % target state +V = 30; % Velocity module + +% Initial guesses + +tf_guess = 40; % initial guess for final time +x_guess = linspace(x0, xf, N+1); % straight line +y_guess = linspace(y0, yf, N+1); % straight line +alpha_guess = atan2(diff(y_guess), diff(x_guess)); +alpha_guess = [alpha_guess, alpha_guess(end)]; % N+1 + +z0 = [x_guess, y_guess, alpha_guess, tf_guess]'; % column + +% bounds for variables + +lb = [-inf*ones(3*(N+1),1); 1]; % tf ≥ 1 s +ub = [ inf*ones(2*(N+1),1); pi*ones(N+1,1); 1e4]; % tf ≤ 10 000 s + +% fmincon solver options + +opts = optimoptions('fmincon','Display','iter','MaxFunEvals',1e6,... + 'MaxIter',1e3,'Algorithm','interior-point'); + +% solve NLP problem using fmincon + +z_opt = fmincon(@objective, z0, [], [], [], [], ... + lb, ub, @(z) constraints(z, xf, yf,V), opts); + +% plot result + +np1 = (length(z_opt)-1)/3; +x = z_opt(1:np1); +y = z_opt(np1+1:2*np1); +alpha = z_opt(2*np1+1:3*np1); +tf = z_opt(end); +t = linspace(0, tf, np1); + +set(groot, 'defaultTextInterpreter', 'latex'); +set(groot, 'defaultAxesTickLabelInterpreter', 'latex'); +set(groot, 'defaultLegendInterpreter', 'latex'); + +figure; +plot(x, y, 'b.-'); +xlabel('x'); ylabel('y'); grid on; +title('Optimal path'); + +alpha_deg = rad2deg(wrapToPi(alpha)); + +figure; +plot(t, alpha_deg, 'r.-','LineWidth', 2, 'MarkerSize', 10); +xlabel('Time [s]'); +ylabel('$\alpha$ $[^\circ]$'); +grid on; +title('Optimal heading angle'); + +% --- Trajectory plot --- +figure; +plot(x, y, 'b.-', 'LineWidth', 2, 'MarkerSize', 10); +hold on; +plot(x(1), y(1), 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % Start +plot(x(end), y(end), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % Target +grid on; +xlabel('x [m]'); +ylabel('y [m]'); +ylim([0 250]) +xlim([0 1000]) +title('Optimal Trajectory using Direct Collocation'); +legend('Trajectory', 'Start', 'Target', 'Location', 'best'); + +% cost function (minimize final time) + +function J = objective(z) + J = z(end); +end + +% constraints +function [c, ceq] = constraints(z, xf, yf,V) + + % decode decision vector + np1 = (length(z)-1)/3; % np1 = N + 1 + N = np1 - 1; % number of intervals + + x = z(1:np1); + y = z(np1+1:2*np1); + alpha = z(2*np1+1:3*np1); + tf = z(end); + h = tf / N; + + % trapezoidal dynamics + ceq_dyn = zeros(2*N,1); + for i = 1:N + f1_i = V*cos(alpha(i)) + 0.5*y(i); + f1_ip1 = V*cos(alpha(i+1)) + 0.5*y(i+1); + + f2_i = V*sin(alpha(i)); + f2_ip1 = V*sin(alpha(i+1)); + + ceq_dyn(i) = x(i+1) - x(i) - h/2*(f1_i + f1_ip1); + ceq_dyn(N+i) = y(i+1) - y(i) - h/2*(f2_i + f2_ip1); + end + + % boundary conditions + ceq_bc = [x(1); y(1); x(end)-xf; y(end)-yf]; + + % Put together all the constraints + ceq = [ceq_dyn; ceq_bc]; % equality constraints + c = []; % no inequality constraints +end diff --git a/OptimalControl/old/cesc/fm_mex_source_zermelo.mexw64 b/OptimalControl/old/cesc/fm_mex_source_zermelo.mexw64 new file mode 100644 index 0000000000..e4d87a7795 Binary files /dev/null and b/OptimalControl/old/cesc/fm_mex_source_zermelo.mexw64 differ diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/_clang-format b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/_clang-format new file mode 100644 index 0000000000..55a16f7ec3 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/_clang-format @@ -0,0 +1,27 @@ +BasedOnStyle: LLVM +Language: Cpp +IndentWidth: 2 +ColumnLimit: 80 +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +CommentPragmas: '^(Return Type|Arguments)\s*' +DeriveLineEnding : true +BreakBeforeBraces: Custom +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: true + AfterFunction: true + AfterNamespace: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + SplitEmptyFunction: true + SplitEmptyRecord: true diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/buildInfo.mat b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/buildInfo.mat new file mode 100644 index 0000000000..131a6a19dc Binary files /dev/null and b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/buildInfo.mat differ diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/codeInfo.mat b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/codeInfo.mat new file mode 100644 index 0000000000..e42dcc8db8 Binary files /dev/null and b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/codeInfo.mat differ diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/codedescriptor.dmr b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/codedescriptor.dmr new file mode 100644 index 0000000000..07722c0c3f Binary files /dev/null and b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/codedescriptor.dmr differ diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/compileInfo.mat b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/compileInfo.mat new file mode 100644 index 0000000000..6eb9432bea Binary files /dev/null and b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/compileInfo.mat differ diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/examples/main.cpp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/examples/main.cpp new file mode 100644 index 0000000000..2de50f6beb --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/examples/main.cpp @@ -0,0 +1,86 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// main.cpp +// +// Code generation for function 'main' +// + +/*************************************************************************/ +/* This automatically generated example C++ main file shows how to call */ +/* entry-point functions that MATLAB Coder generated. You must customize */ +/* this file for your application. Do not modify this file directly. */ +/* Instead, make a copy of this file, modify it, and integrate it into */ +/* your development environment. */ +/* */ +/* This file initializes entry-point function arguments to a default */ +/* size and value before calling the entry-point functions. It does */ +/* not store or use any values returned from the entry-point functions. */ +/* If necessary, it does pre-allocate memory for returned values. */ +/* You can use this file as a starting point for a main function that */ +/* you can deploy in your application. */ +/* */ +/* After you copy the file, and before you deploy it, you must make the */ +/* following changes: */ +/* * For variable-size function arguments, change the example sizes to */ +/* the sizes that your application requires. */ +/* * Change the example values of function arguments to the values that */ +/* your application requires. */ +/* * If the entry-point functions return values, store these values or */ +/* otherwise use them as required by your application. */ +/* */ +/*************************************************************************/ + +// Include files +#include "main.h" +#include "mb_fm_mex_source_zermelo.h" + +// Function Declarations +static void argInit_2x1_real_T(double result[2]); + +static double argInit_real_T(); + +// Function Definitions +static void argInit_2x1_real_T(double result[2]) +{ + // Loop over the array to initialize each element. + for (int idx0{0}; idx0 < 2; idx0++) { + // Set the value of the array element. + // Change this value to the value that the application requires. + result[idx0] = argInit_real_T(); + } +} + +static double argInit_real_T() +{ + return 0.0; +} + +int main(int, char **) +{ + // The initialize function is being called automatically from your entry-point + // function. So, a call to initialize is not included here. Invoke the + // entry-point functions. + // You can call entry-point functions multiple times. + main_mb_fm_mex_source_zermelo(); + // Terminate the application. + // You do not need to do this more than one time. + mb_fm_mex_source_zermelo_terminate(); + return 0; +} + +void main_mb_fm_mex_source_zermelo() +{ + double j_statesdot[6]; + double dv[2]; + double statesdot[2]; + // Initialize function 'mb_fm_mex_source_zermelo' input arguments. + // Initialize function input argument 'states'. + // Call the entry-point 'mb_fm_mex_source_zermelo'. + argInit_2x1_real_T(dv); + mb_fm_mex_source_zermelo(dv, argInit_real_T(), statesdot, j_statesdot); +} + +// End of code generation (main.cpp) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/examples/main.h b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/examples/main.h new file mode 100644 index 0000000000..ab3513fdb2 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/examples/main.h @@ -0,0 +1,54 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// main.h +// +// Code generation for function 'main' +// + +/*************************************************************************/ +/* This automatically generated example C++ main file shows how to call */ +/* entry-point functions that MATLAB Coder generated. You must customize */ +/* this file for your application. Do not modify this file directly. */ +/* Instead, make a copy of this file, modify it, and integrate it into */ +/* your development environment. */ +/* */ +/* This file initializes entry-point function arguments to a default */ +/* size and value before calling the entry-point functions. It does */ +/* not store or use any values returned from the entry-point functions. */ +/* If necessary, it does pre-allocate memory for returned values. */ +/* You can use this file as a starting point for a main function that */ +/* you can deploy in your application. */ +/* */ +/* After you copy the file, and before you deploy it, you must make the */ +/* following changes: */ +/* * For variable-size function arguments, change the example sizes to */ +/* the sizes that your application requires. */ +/* * Change the example values of function arguments to the values that */ +/* your application requires. */ +/* * If the entry-point functions return values, store these values or */ +/* otherwise use them as required by your application. */ +/* */ +/*************************************************************************/ + +#ifndef MAIN_H +#define MAIN_H + +// Include files +#include "rtwtypes.h" +#include +#include + +// Custom Header Code +#include +#define CHAR16_T uint16_t +#include "mex.h" +// Function Declarations +extern int main(int argc, char **argv); + +extern void main_mb_fm_mex_source_zermelo(); + +#endif +// End of code generation (main.h) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_api.cpp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_api.cpp new file mode 100644 index 0000000000..d532773afd --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_api.cpp @@ -0,0 +1,214 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// _coder_mb_fm_mex_source_zermelo_api.cpp +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +// Include files +#include "_coder_mb_fm_mex_source_zermelo_api.h" +#include "_coder_mb_fm_mex_source_zermelo_mex.h" + +// Variable Definitions +emlrtCTX emlrtRootTLSGlobal{nullptr}; + +emlrtContext emlrtContextGlobal{ + true, // bFirstTime + false, // bInitialized + 131642U, // fVersionInfo + nullptr, // fErrorFunction + "mb_fm_mex_source_zermelo", // fFunctionName + nullptr, // fRTCallStack + false, // bDebugMode + {2045744189U, 2170104910U, 2743257031U, 4284093946U}, // fSigWrd + nullptr // fSigMem +}; + +// Function Declarations +static real_T b_emlrt_marshallIn(const emlrtStack &sp, const mxArray *controls, + const char_T *identifier); + +static real_T b_emlrt_marshallIn(const emlrtStack &sp, const mxArray *u, + const emlrtMsgIdentifier *parentId); + +static const mxArray *b_emlrt_marshallOut(const real_T u[6]); + +static real_T (*c_emlrt_marshallIn(const emlrtStack &sp, const mxArray *src, + const emlrtMsgIdentifier *msgId))[2]; + +static real_T d_emlrt_marshallIn(const emlrtStack &sp, const mxArray *src, + const emlrtMsgIdentifier *msgId); + +static real_T (*emlrt_marshallIn(const emlrtStack &sp, const mxArray *states, + const char_T *identifier))[2]; + +static real_T (*emlrt_marshallIn(const emlrtStack &sp, const mxArray *u, + const emlrtMsgIdentifier *parentId))[2]; + +static const mxArray *emlrt_marshallOut(const real_T u[2]); + +// Function Definitions +static real_T b_emlrt_marshallIn(const emlrtStack &sp, const mxArray *controls, + const char_T *identifier) +{ + emlrtMsgIdentifier thisId; + real_T y; + thisId.fIdentifier = const_cast(identifier); + thisId.fParent = nullptr; + thisId.bParentIsCell = false; + y = b_emlrt_marshallIn(sp, emlrtAlias(controls), &thisId); + emlrtDestroyArray(&controls); + return y; +} + +static real_T b_emlrt_marshallIn(const emlrtStack &sp, const mxArray *u, + const emlrtMsgIdentifier *parentId) +{ + real_T y; + y = d_emlrt_marshallIn(sp, emlrtAlias(u), parentId); + emlrtDestroyArray(&u); + return y; +} + +static const mxArray *b_emlrt_marshallOut(const real_T u[6]) +{ + static const int32_T iv[2]{0, 0}; + static const int32_T iv1[2]{2, 3}; + const mxArray *m; + const mxArray *y; + y = nullptr; + m = emlrtCreateNumericArray(2, (const void *)&iv[0], mxDOUBLE_CLASS, mxREAL); + emlrtMxSetData((mxArray *)m, (void *)&u[0]); + emlrtSetDimensions((mxArray *)m, &iv1[0], 2); + emlrtAssign(&y, m); + return y; +} + +static real_T (*c_emlrt_marshallIn(const emlrtStack &sp, const mxArray *src, + const emlrtMsgIdentifier *msgId))[2] +{ + static const int32_T dims{2}; + real_T(*ret)[2]; + int32_T i; + boolean_T b{false}; + emlrtCheckVsBuiltInR2012b((emlrtConstCTX)&sp, msgId, src, "double", false, 1U, + (const void *)&dims, &b, &i); + ret = (real_T(*)[2])emlrtMxGetData(src); + emlrtDestroyArray(&src); + return ret; +} + +static real_T d_emlrt_marshallIn(const emlrtStack &sp, const mxArray *src, + const emlrtMsgIdentifier *msgId) +{ + static const int32_T dims{0}; + real_T ret; + emlrtCheckBuiltInR2012b((emlrtConstCTX)&sp, msgId, src, "double", false, 0U, + (const void *)&dims); + ret = *static_cast(emlrtMxGetData(src)); + emlrtDestroyArray(&src); + return ret; +} + +static real_T (*emlrt_marshallIn(const emlrtStack &sp, const mxArray *states, + const char_T *identifier))[2] +{ + emlrtMsgIdentifier thisId; + real_T(*y)[2]; + thisId.fIdentifier = const_cast(identifier); + thisId.fParent = nullptr; + thisId.bParentIsCell = false; + y = emlrt_marshallIn(sp, emlrtAlias(states), &thisId); + emlrtDestroyArray(&states); + return y; +} + +static real_T (*emlrt_marshallIn(const emlrtStack &sp, const mxArray *u, + const emlrtMsgIdentifier *parentId))[2] +{ + real_T(*y)[2]; + y = c_emlrt_marshallIn(sp, emlrtAlias(u), parentId); + emlrtDestroyArray(&u); + return y; +} + +static const mxArray *emlrt_marshallOut(const real_T u[2]) +{ + static const int32_T i{0}; + static const int32_T i1{2}; + const mxArray *m; + const mxArray *y; + y = nullptr; + m = emlrtCreateNumericArray(1, (const void *)&i, mxDOUBLE_CLASS, mxREAL); + emlrtMxSetData((mxArray *)m, (void *)&u[0]); + emlrtSetDimensions((mxArray *)m, &i1, 1); + emlrtAssign(&y, m); + return y; +} + +void mb_fm_mex_source_zermelo_api(const mxArray *const prhs[2], int32_T nlhs, + const mxArray *plhs[2]) +{ + emlrtStack st{ + nullptr, // site + nullptr, // tls + nullptr // prev + }; + real_T(*j_statesdot)[6]; + real_T(*states)[2]; + real_T(*statesdot)[2]; + real_T controls; + st.tls = emlrtRootTLSGlobal; + statesdot = (real_T(*)[2])mxMalloc(sizeof(real_T[2])); + j_statesdot = (real_T(*)[6])mxMalloc(sizeof(real_T[6])); + // Marshall function inputs + states = emlrt_marshallIn(st, emlrtAlias(prhs[0]), "states"); + controls = b_emlrt_marshallIn(st, emlrtAliasP(prhs[1]), "controls"); + // Invoke the target function + mb_fm_mex_source_zermelo(*states, controls, *statesdot, *j_statesdot); + // Marshall function outputs + plhs[0] = emlrt_marshallOut(*statesdot); + if (nlhs > 1) { + plhs[1] = b_emlrt_marshallOut(*j_statesdot); + } +} + +void mb_fm_mex_source_zermelo_atexit() +{ + emlrtStack st{ + nullptr, // site + nullptr, // tls + nullptr // prev + }; + mexFunctionCreateRootTLS(); + st.tls = emlrtRootTLSGlobal; + emlrtEnterRtStackR2012b(&st); + emlrtDestroyRootTLS(&emlrtRootTLSGlobal); + mb_fm_mex_source_zermelo_xil_terminate(); + mb_fm_mex_source_zermelo_xil_shutdown(); + emlrtExitTimeCleanup(&emlrtContextGlobal); +} + +void mb_fm_mex_source_zermelo_initialize() +{ + emlrtStack st{ + nullptr, // site + nullptr, // tls + nullptr // prev + }; + mexFunctionCreateRootTLS(); + st.tls = emlrtRootTLSGlobal; + emlrtClearAllocCountR2012b(&st, false, 0U, nullptr); + emlrtEnterRtStackR2012b(&st); + emlrtFirstTimeR2012b(emlrtRootTLSGlobal); +} + +void mb_fm_mex_source_zermelo_terminate() +{ + emlrtDestroyRootTLS(&emlrtRootTLSGlobal); +} + +// End of code generation (_coder_mb_fm_mex_source_zermelo_api.cpp) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_api.h b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_api.h new file mode 100644 index 0000000000..5eb4a9a85c --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_api.h @@ -0,0 +1,42 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// _coder_mb_fm_mex_source_zermelo_api.h +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +#ifndef _CODER_MB_FM_MEX_SOURCE_ZERMELO_API_H +#define _CODER_MB_FM_MEX_SOURCE_ZERMELO_API_H + +// Include files +#include "emlrt.h" +#include "tmwtypes.h" +#include +#include + +// Variable Declarations +extern emlrtCTX emlrtRootTLSGlobal; +extern emlrtContext emlrtContextGlobal; + +// Function Declarations +void mb_fm_mex_source_zermelo(real_T states[2], real_T controls, + real_T statesdot[2], real_T j_statesdot[6]); + +void mb_fm_mex_source_zermelo_api(const mxArray *const prhs[2], int32_T nlhs, + const mxArray *plhs[2]); + +void mb_fm_mex_source_zermelo_atexit(); + +void mb_fm_mex_source_zermelo_initialize(); + +void mb_fm_mex_source_zermelo_terminate(); + +void mb_fm_mex_source_zermelo_xil_shutdown(); + +void mb_fm_mex_source_zermelo_xil_terminate(); + +#endif +// End of code generation (_coder_mb_fm_mex_source_zermelo_api.h) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_info.cpp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_info.cpp new file mode 100644 index 0000000000..8392721487 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_info.cpp @@ -0,0 +1,84 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// _coder_mb_fm_mex_source_zermelo_info.cpp +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +// Include files +#include "_coder_mb_fm_mex_source_zermelo_info.h" +#include "emlrt.h" +#include "tmwtypes.h" + +// Function Declarations +static const mxArray *emlrtMexFcnResolvedFunctionsInfo(); + +// Function Definitions +static const mxArray *emlrtMexFcnResolvedFunctionsInfo() +{ + const mxArray *nameCaptureInfo; + const char_T *data[5]{ + "789cc554cb4ec240141d0c3e36282bbfc19569e55170a5a13e13450326268e29657aabd5" + "ce0ce9b408ee4ddcb9d2cf72e1d6855b3fc1a5522850e2041222decd" + "9d93939973ee99c9a0c4c1510221b48cba955eecf654847b7d0ec56b944f487a54f32819" + "db17f14fbd4e38f3a1e577013329f4775a9c3acc647eb5dd00e481e0", + "6e13ac90b11d17aa0e85ca3038ee20ba3b44f54187eaac4bd7406e2b0145deb518387487" + "413f8f4fc9bcc909f37890e4911ee12f762e4b9bf84c802770cb6c3a" + "0d5c66a07b4e13b0ce494081f95ce03dc7df0feab87267325c6ef80e35ddd24f701e7731" + "0141b04d0dca2d7045b882962178e01130eec1a3e0726c39b68d69dd", + "f8955da743733f4e39f7da98b9235eb485912916c056f2d9a2aa65b24a319f2b281a5188" + "aadaaaa98169193726897cd5a6f4b520f5d5652c1ed45d18e4f035a5" + "deb3542fcecff0fe278d3cf61e6a92395726cc41f62fa4d052d85f5edf436a567a871f5b" + "6fb3d48beabff45a92f3267dc7ab12bdf408af072ca7f1ea39dbe679", + "ce3772ea15390df4818f93313ae37c2009feebf3bf012de1b6d5", ""}; + nameCaptureInfo = nullptr; + emlrtNameCaptureMxArrayR2016a(&data[0], 1832U, &nameCaptureInfo); + return nameCaptureInfo; +} + +mxArray *emlrtMexFcnProperties() +{ + mxArray *xEntryPoints; + mxArray *xInputs; + mxArray *xResult; + const char_T *propFieldName[7]{ + "Version", "ResolvedFunctions", "Checksum", "EntryPoints", + "CoverageInfo", "IsPolymorphic", "PropertyList"}; + const char_T *epFieldName[6]{ + "Name", "NumberOfInputs", "NumberOfOutputs", + "ConstantInputs", "FullPath", "TimeStamp"}; + xEntryPoints = + emlrtCreateStructMatrix(1, 1, 6, (const char_T **)&epFieldName[0]); + xInputs = emlrtCreateLogicalMatrix(1, 2); + emlrtSetField(xEntryPoints, 0, "Name", + emlrtMxCreateString("mb_fm_mex_source_zermelo")); + emlrtSetField(xEntryPoints, 0, "NumberOfInputs", + emlrtMxCreateDoubleScalar(2.0)); + emlrtSetField(xEntryPoints, 0, "NumberOfOutputs", + emlrtMxCreateDoubleScalar(2.0)); + emlrtSetField(xEntryPoints, 0, "ConstantInputs", xInputs); + emlrtSetField( + xEntryPoints, 0, "FullPath", + emlrtMxCreateString( + "C:" + "\\Users\\xavip\\OneDrive\\Documentos\\GitHub\\Swan\\OptimalControl\\" + "cesc\\fm_models\\fm_mex_source_zermelo\\diff\\mb_fm_mex_so" + "urce_zermelo.m")); + emlrtSetField(xEntryPoints, 0, "TimeStamp", + emlrtMxCreateDoubleScalar(739764.06609953707)); + xResult = + emlrtCreateStructMatrix(1, 1, 7, (const char_T **)&propFieldName[0]); + emlrtSetField(xResult, 0, "Version", + emlrtMxCreateString("9.14.0.2891782 (R2023a) Update 8")); + emlrtSetField(xResult, 0, "ResolvedFunctions", + (mxArray *)emlrtMexFcnResolvedFunctionsInfo()); + emlrtSetField(xResult, 0, "Checksum", + emlrtMxCreateString("P2BaLQnArfOPfnWYWVXwt")); + emlrtSetField(xResult, 0, "EntryPoints", xEntryPoints); + return xResult; +} + +// End of code generation (_coder_mb_fm_mex_source_zermelo_info.cpp) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_info.h b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_info.h new file mode 100644 index 0000000000..3df024048d --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_info.h @@ -0,0 +1,21 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// _coder_mb_fm_mex_source_zermelo_info.h +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +#ifndef _CODER_MB_FM_MEX_SOURCE_ZERMELO_INFO_H +#define _CODER_MB_FM_MEX_SOURCE_ZERMELO_INFO_H + +// Include files +#include "mex.h" + +// Function Declarations +MEXFUNCTION_LINKAGE mxArray *emlrtMexFcnProperties(); + +#endif +// End of code generation (_coder_mb_fm_mex_source_zermelo_info.h) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_mex.cpp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_mex.cpp new file mode 100644 index 0000000000..55081ff3a1 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_mex.cpp @@ -0,0 +1,70 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// _coder_mb_fm_mex_source_zermelo_mex.cpp +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +// Include files +#include "_coder_mb_fm_mex_source_zermelo_mex.h" +#include "_coder_mb_fm_mex_source_zermelo_api.h" + +// Function Definitions +void mexFunction(int32_T nlhs, mxArray *plhs[], int32_T nrhs, + const mxArray *prhs[]) +{ + mexAtExit(&mb_fm_mex_source_zermelo_atexit); + // Module initialization. + mb_fm_mex_source_zermelo_initialize(); + // Dispatch the entry-point. + unsafe_mb_fm_mex_source_zermelo_mexFunction(nlhs, plhs, nrhs, prhs); + // Module termination. + mb_fm_mex_source_zermelo_terminate(); +} + +emlrtCTX mexFunctionCreateRootTLS() +{ + emlrtCreateRootTLSR2022a(&emlrtRootTLSGlobal, &emlrtContextGlobal, nullptr, 1, + nullptr, "windows-1252", true); + return emlrtRootTLSGlobal; +} + +void unsafe_mb_fm_mex_source_zermelo_mexFunction(int32_T nlhs, mxArray *plhs[2], + int32_T nrhs, + const mxArray *prhs[2]) +{ + emlrtStack st{ + nullptr, // site + nullptr, // tls + nullptr // prev + }; + const mxArray *b_prhs[2]; + const mxArray *outputs[2]; + int32_T i; + st.tls = emlrtRootTLSGlobal; + // Check for proper number of arguments. + if (nrhs != 2) { + emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:WrongNumberOfInputs", 5, 12, 2, 4, + 24, "mb_fm_mex_source_zermelo"); + } + if (nlhs > 2) { + emlrtErrMsgIdAndTxt(&st, "EMLRT:runTime:TooManyOutputArguments", 3, 4, 24, + "mb_fm_mex_source_zermelo"); + } + // Call the function. + b_prhs[0] = prhs[0]; + b_prhs[1] = prhs[1]; + mb_fm_mex_source_zermelo_api(b_prhs, nlhs, outputs); + // Copy over outputs to the caller. + if (nlhs < 1) { + i = 1; + } else { + i = nlhs; + } + emlrtReturnArrays(i, &plhs[0], &outputs[0]); +} + +// End of code generation (_coder_mb_fm_mex_source_zermelo_mex.cpp) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_mex.h b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_mex.h new file mode 100644 index 0000000000..1b1d75ac81 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/interface/_coder_mb_fm_mex_source_zermelo_mex.h @@ -0,0 +1,30 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// _coder_mb_fm_mex_source_zermelo_mex.h +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +#ifndef _CODER_MB_FM_MEX_SOURCE_ZERMELO_MEX_H +#define _CODER_MB_FM_MEX_SOURCE_ZERMELO_MEX_H + +// Include files +#include "emlrt.h" +#include "mex.h" +#include "tmwtypes.h" + +// Function Declarations +MEXFUNCTION_LINKAGE void mexFunction(int32_T nlhs, mxArray *plhs[], + int32_T nrhs, const mxArray *prhs[]); + +emlrtCTX mexFunctionCreateRootTLS(); + +void unsafe_mb_fm_mex_source_zermelo_mexFunction(int32_T nlhs, mxArray *plhs[2], + int32_T nrhs, + const mxArray *prhs[2]); + +#endif +// End of code generation (_coder_mb_fm_mex_source_zermelo_mex.h) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo.cpp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo.cpp new file mode 100644 index 0000000000..0c67e72878 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo.cpp @@ -0,0 +1,98 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// mb_fm_mex_source_zermelo.cpp +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +// Include files +#include "mb_fm_mex_source_zermelo.h" +#include + +// Function Definitions +void mb_fm_mex_source_zermelo(const double states[2], double controls, + double statesdot[2], double j_statesdot[6]) +{ + static const signed char iv[3]{0, 0, 1}; + double b_j_states[9]; + double dv[6]; + double j_statedottmp[6]; + double out1_tmp; + double statesdot_tmp; + int j_states_tmp; + signed char j_states[6]; + // mb_fm_mex_source_zermelo + // File automatically generated by FALCON.m + // === Extract Data From Input ============================================== + // === Jacobians and Hessians =============================================== + for (int i{0}; i < 6; i++) { + j_states[i] = 0; + } + j_states[0] = 1; + j_states[1] = 0; + j_states[2] = 0; + j_states[3] = 1; + // === Write Constants ====================================================== + // === Subsystem source_zermelo ============================================= + // Defined at ? + // Call sys_398ef064917340965807c0c11f1a7ead_jac + // SYS_398EF064917340965807C0C11F1A7EAD_JAC + // [OUT1,J_OUT1,H_OUT1] = + // SYS_398EF064917340965807C0C11F1A7EAD_JAC(IN1,IN2VAL1) This function was + // generated by the Symbolic Math Toolbox version 9.3. 27-May-2025 + // 01:35:10 + // Generated using CreateGradient + // Input dimensions : {[2 1], [1 1]} + // Derivative inputs: [true true] + // DerivativeOrder : 1 + // Output dimensions: {[2 1]} + // Constant outputs : false + out1_tmp = std::sin(controls); + statesdot_tmp = std::cos(controls) * 30.0; + statesdot[0] = states[1] / 2.0 + statesdot_tmp; + statesdot[1] = out1_tmp * 30.0; + // Hessian Jacobian for sys_398ef064917340965807c0c11f1a7ead_jac + // Calculation of Jacobian with respect to function global input for + // sys_398ef064917340965807c0c11f1a7ead_jac + dv[0] = 0.0; + dv[1] = 0.0; + dv[2] = 0.5; + dv[3] = 0.0; + dv[4] = out1_tmp * -30.0; + dv[5] = statesdot_tmp; + for (int i{0}; i < 3; i++) { + j_states_tmp = i << 1; + b_j_states[3 * i] = j_states[j_states_tmp]; + b_j_states[3 * i + 1] = j_states[j_states_tmp + 1]; + b_j_states[3 * i + 2] = iv[i]; + } + for (int i{0}; i < 2; i++) { + out1_tmp = dv[i + 2]; + statesdot_tmp = dv[i + 4]; + for (j_states_tmp = 0; j_states_tmp < 3; j_states_tmp++) { + j_statedottmp[i + (j_states_tmp << 1)] = + out1_tmp * b_j_states[3 * j_states_tmp + 1] + + statesdot_tmp * b_j_states[3 * j_states_tmp + 2]; + } + } + // Combine Variables to statesdot + j_statesdot[0] = j_statedottmp[0]; + j_statesdot[1] = j_statedottmp[1]; + j_statesdot[2] = j_statedottmp[2]; + j_statesdot[3] = j_statedottmp[3]; + j_statesdot[4] = j_statedottmp[4]; + j_statesdot[5] = j_statedottmp[5]; +} + +void mb_fm_mex_source_zermelo_initialize() +{ +} + +void mb_fm_mex_source_zermelo_terminate() +{ +} + +// End of code generation (mb_fm_mex_source_zermelo.cpp) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo.h b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo.h new file mode 100644 index 0000000000..e1e9eedecf --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo.h @@ -0,0 +1,33 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// mb_fm_mex_source_zermelo.h +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +#ifndef MB_FM_MEX_SOURCE_ZERMELO_H +#define MB_FM_MEX_SOURCE_ZERMELO_H + +// Include files +#include "rtwtypes.h" +#include +#include + +// Custom Header Code +#include +#define CHAR16_T uint16_t +#include "mex.h" +// Function Declarations +extern void mb_fm_mex_source_zermelo(const double states[2], double controls, + double statesdot[2], + double j_statesdot[6]); + +extern void mb_fm_mex_source_zermelo_initialize(); + +extern void mb_fm_mex_source_zermelo_terminate(); + +#endif +// End of code generation (mb_fm_mex_source_zermelo.h) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_CALL.cpp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_CALL.cpp new file mode 100644 index 0000000000..ad2c2d2777 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_CALL.cpp @@ -0,0 +1,211 @@ +// Call Function generated automatically on 27-May-2025 01:35:29 +// Created on DESKTOP-BM120QM +#include "cstdint" +#include "mex.h" +#include "math.h" +#include "algorithm" +#include "array" +#include "mb_fm_mex_source_zermelo.h" +#include "mb_fm_mex_source_zermelo_types.h" + +// Constants - Input Dimensions +#define DIM_M_STATES 2 +#define DIM_M_CONTROLS 1 + +// Constants - Number of Independent Variables (non discrete control case) +#define N_IDP 3 + +// Constants - Output Sizes +#define NUM_OUT_STATESDOT 2 + +static int const nthreads = 1; + +// Function Header +void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) try { + + // Declare input arrays + mxArray const *array_states = prhs[0]; + mxArray const *array_controls = prhs[1]; + + // Declare number of time steps + int nEval; + + // Declare Outputs + double *statesdot; + double *j_statesdot; + mwSize j_dim_statesdot[3]; + + // Write Call Information + if (nrhs == 0 && nlhs == 0) { + // System Information + mexPrintf("System Information\n"); + mexPrintf("Mex file created by Derivative Model Builder\n"); + mexPrintf("- Date: 27-May-2025 01:35:29\n"); + mexPrintf("- Computer: DESKTOP-BM120QM\n"); + mexPrintf("- MATLAB-Version: 9.14.0.2891782 (R2023a) Update 8\n"); + mexPrintf("- DerivativeOrder: 1\n"); + mexPrintf("- Jacobian Calculation:true\n"); + mexPrintf("- Hessian-Calculation: false\n"); + mexPrintf("\n"); + + // Input Information + mexPrintf("Input Information\n"); + mexPrintf("Name Size DataType Derivative MultipleTimeEval VariableSize\n"); + mexPrintf("states [2 1] double true true false\n"); + mexPrintf("| x\n"); + mexPrintf("| y\n"); + mexPrintf("\n"); + mexPrintf("controls [1 1] double true true false\n"); + mexPrintf("| u\n"); + mexPrintf("\n"); + mexPrintf("\n"); + + // Output Information + mexPrintf("Output Information\n"); + mexPrintf("Name Size \n"); + mexPrintf("statesdot [2 1]\n"); + + return; + } + else if (nrhs == 0 && nlhs == 1) { + const char *names[] = {"input", "output", "info", "name", "type", "WrapperClass"}; + const char *i_names[] = {"m", "n","name","argnames","type", "groupindex", "DataType"}; + const char *o_names[] = {"m", "n","name","argnames","type","jac_sparsity","hess_sparsity"}; + mxArray *struct_inputs; + mxArray *struct_outputs; + const char *info_names[] = {"Date", "Computer", "MATLAB", "DerivativeOrder", "Jacobian", "Hessian", "UseSparsityEstimator"}; + mxArray *struct_info; + mxArray *mx; + double *sparsity_j; + double *sparsity_h; + + plhs[0] = mxCreateStructMatrix(1,1,6,names); + mxSetField(plhs[0], 0, names[3], mxCreateString("fm_mex_source_zermelo")); + mxSetField(plhs[0], 0, names[4], mxCreateString("SIMULATION_MODEL")); + mxSetField(plhs[0], 0, names[5], mxCreateString("falcon.ModelWrapper")); + + struct_inputs = mxCreateStructMatrix(2,1,7,i_names); + + mxSetField(struct_inputs, 0, i_names[0], mxCreateDoubleScalar(2)); + mxSetField(struct_inputs, 0, i_names[1], mxCreateDoubleScalar(1)); + mxSetField(struct_inputs, 0, i_names[2], mxCreateString("states")); + mxSetField(struct_inputs, 0, i_names[4], mxCreateString("STATE")); + mxSetField(struct_inputs, 0, i_names[5], mxCreateDoubleScalar(0)); + mxSetField(struct_inputs, 0, i_names[6], mxCreateString("double")); + mx = mxCreateCellMatrix(2, 1); + mxSetCell(mx, 0, mxCreateString("x")); + mxSetCell(mx, 1, mxCreateString("y")); + mxSetField(struct_inputs, 0, i_names[3], mx); + + mxSetField(struct_inputs, 1, i_names[0], mxCreateDoubleScalar(1)); + mxSetField(struct_inputs, 1, i_names[1], mxCreateDoubleScalar(1)); + mxSetField(struct_inputs, 1, i_names[2], mxCreateString("controls")); + mxSetField(struct_inputs, 1, i_names[4], mxCreateString("CONTROL")); + mxSetField(struct_inputs, 1, i_names[5], mxCreateDoubleScalar(0)); + mxSetField(struct_inputs, 1, i_names[6], mxCreateString("double")); + mx = mxCreateCellMatrix(1, 1); + mxSetCell(mx, 0, mxCreateString("u")); + mxSetField(struct_inputs, 1, i_names[3], mx); + mxSetField(plhs[0], 0, names[0], struct_inputs); + + struct_outputs = mxCreateStructMatrix(1,1,7,o_names); + + mxSetField(struct_outputs, 0, o_names[0], mxCreateDoubleScalar(2)); + mxSetField(struct_outputs, 0, o_names[1], mxCreateDoubleScalar(1)); + mxSetField(struct_outputs, 0, o_names[2], mxCreateString("statesdot")); + mxSetField(struct_outputs, 0, o_names[4], mxCreateString("STATEDOT")); + mx = mxCreateDoubleMatrix(2, 3, mxREAL); + sparsity_j = reinterpret_cast(mxGetPr(mx)); + { + std::array const sparsity_j_template = { + 0, 0, /* column 1 */ + 1, 0, /* column 2 */ + 1, 1, /* column 3 */ + }; /* end of sparsity_j_template */ + std::copy(sparsity_j_template.cbegin(), sparsity_j_template.cend(), sparsity_j); + } + mxSetField(struct_outputs, 0, o_names[5], mx); + mx = mxCreateDoubleMatrix(0, 0, mxREAL); + sparsity_h = reinterpret_cast(mxGetPr(mx)); + { + std::array const sparsity_h_template = { + }; /* end of sparsity_h_template */ + std::copy(sparsity_h_template.cbegin(), sparsity_h_template.cend(), sparsity_h); + } + mxSetField(struct_outputs, 0, o_names[6], mx); + mx = mxCreateCellMatrix(0,0); + mxSetField(struct_outputs, 0, o_names[3], mx); + mxSetField(plhs[0], 0, names[1], struct_outputs); + struct_info = mxCreateStructMatrix(1,1,7,info_names); + mxSetField(struct_info, 0, info_names[0], mxCreateString("27-May-2025 01:35:29")); + mxSetField(struct_info, 0, info_names[1], mxCreateString("DESKTOP-BM120QM")); + mxSetField(struct_info, 0, info_names[2], mxCreateString("9.14.0.2891782 (R2023a) Update 8")); + mxSetField(struct_info, 0, info_names[3], mxCreateDoubleScalar(1)); + mxSetField(struct_info, 0, info_names[4], mxCreateLogicalScalar(true)); + mxSetField(struct_info, 0, info_names[5], mxCreateLogicalScalar(false)); + mxSetField(struct_info, 0, info_names[6], mxCreateLogicalScalar(false)); + mxSetField(plhs[0], 0, names[2], struct_info); + return; + } + + // Check Number of Input Arguments + if (nrhs < 2 || nrhs > 2) { + mexErrMsgIdAndTxt("MATLAB:callModel:Error","Wrong number of input arguments for fm_mex_source_zermelo. The required number of input arguments is 2. Call the mex file with no input arguments to get exact input & output type information."); + } + + // Extract Inputs + + if (!mxIsClass(array_states, "double") || mxIsComplex(array_states)) { + mexErrMsgIdAndTxt("MATLAB:callModel:Error", "Wrong argument type: states must be real double, found %s.", mxGetClassName(array_states)); + } + double const *states = reinterpret_cast(mxGetPr(array_states)); + + if (!mxIsClass(array_controls, "double") || mxIsComplex(array_controls)) { + mexErrMsgIdAndTxt("MATLAB:callModel:Error", "Wrong argument type: controls must be real double, found %s.", mxGetClassName(array_controls)); + } + double const *controls = reinterpret_cast(mxGetPr(array_controls)); + nEval = mxGetN(array_states); + + // Check Input Dimensions + // Input_1: states + if (mxGetM(array_states) != DIM_M_STATES) { + mexErrMsgIdAndTxt("MATLAB:callModel:Error", "Number of rows for input 1 with name \"states\" should be 2 but is %i.", mxGetM(prhs[0])); + } + if (mxGetN(array_states) != nEval) { + mexErrMsgIdAndTxt("MATLAB:callModel:Error", "Number of columns for input 1 with name \"states\" should be equal to the number of time steps (%i) but is %i.", nEval, mxGetN(prhs[0])); + } + // Input_2: controls + if (mxGetM(array_controls) != DIM_M_CONTROLS) { + mexErrMsgIdAndTxt("MATLAB:callModel:Error", "Number of rows for input 2 with name \"controls\" should be 1 but is %i.", mxGetM(prhs[1])); + } + if (mxGetN(array_controls) != nEval) { + mexErrMsgIdAndTxt("MATLAB:callModel:Error", "Number of columns for input 2 with name \"controls\" should be equal to the number of time steps (%i) but is %i.", nEval, mxGetN(prhs[1])); + } + + // Prepare jacobian and hessian dimensions + // Output_1: statesdot + plhs[0] = mxCreateDoubleMatrix(2, 1*nEval, mxREAL); + statesdot = mxGetPr(plhs[0]); + // Output_jacobian_1: statesdot + j_dim_statesdot[0] = NUM_OUT_STATESDOT; + j_dim_statesdot[1] = N_IDP; + j_dim_statesdot[2] = nEval; + plhs[1] = mxCreateNumericArray(3,j_dim_statesdot,mxDOUBLE_CLASS,mxREAL); + j_statesdot = mxGetPr(plhs[1]); + + // Call Model in for-loop + for (int iEval = 0; iEval < nEval; iEval++) { + mb_fm_mex_source_zermelo( + &states[DIM_M_STATES * iEval], + controls[DIM_M_CONTROLS * iEval], + &statesdot[NUM_OUT_STATESDOT * iEval], + &j_statesdot[NUM_OUT_STATESDOT * N_IDP * iEval] + ); + } + // Remove Temporary Variables - Variable Size Data + +} +catch (std::exception& e) +{ + mexErrMsgIdAndTxt("MATLAB:callModel:Error", "%s", e.what()); +} diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.bat b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.bat new file mode 100644 index 0000000000..cbd33d1349 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.bat @@ -0,0 +1,16 @@ +@echo off + +call "setup_mingw.bat" + +cd . + +chcp 1252 + +if "%1"=="" ("%MINGW_ROOT%\mingw32-make.exe" -f mb_fm_mex_source_zermelo_rtw.mk all) else ("%MINGW_ROOT%\mingw32-make.exe" -f mb_fm_mex_source_zermelo_rtw.mk %1) +@if errorlevel 1 goto error_exit + +exit /B 0 + +:error_exit +echo The make command returned an error of %errorlevel% +exit /B 1 \ No newline at end of file diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.mk b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.mk new file mode 100644 index 0000000000..81aa798135 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.mk @@ -0,0 +1,391 @@ +########################################################################### +## Makefile generated for component 'mb_fm_mex_source_zermelo'. +## +## Makefile : mb_fm_mex_source_zermelo_rtw.mk +## Generated on : Tue May 27 01:35:27 2025 +## Final product: C:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc/mb_fm_mex_source_zermelo.exe +## Product type : executable +## +########################################################################### + +########################################################################### +## MACROS +########################################################################### + +# Macro Descriptions: +# PRODUCT_NAME Name of the system to build +# MAKEFILE Name of this makefile +# COMPILER_COMMAND_FILE Compiler command listing model reference header paths +# CMD_FILE Command file + +PRODUCT_NAME = mb_fm_mex_source_zermelo +MAKEFILE = mb_fm_mex_source_zermelo_rtw.mk +MATLAB_ROOT = C:/PROGRA~1/MATLAB/R2023a +MATLAB_BIN = C:/PROGRA~1/MATLAB/R2023a/bin +MATLAB_ARCH_BIN = $(MATLAB_BIN)/win64 +START_DIR = C:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc/fm_models/fm_mex_source_zermelo/cpp +TGT_FCN_LIB = ISO_C++11 +SOLVER_OBJ = +CLASSIC_INTERFACE = 0 +MODEL_HAS_DYNAMICALLY_LOADED_SFCNS = +RELATIVE_PATH_TO_ANCHOR = . +COMPILER_COMMAND_FILE = mb_fm_mex_source_zermelo_rtw_comp.rsp +CMD_FILE = mb_fm_mex_source_zermelo_rtw.rsp +C_STANDARD_OPTS = -fwrapv +CPP_STANDARD_OPTS = -fwrapv + +########################################################################### +## TOOLCHAIN SPECIFICATIONS +########################################################################### + +# Toolchain Name: MinGW64 | gmake (64-bit Windows) +# Supported Version(s): 6.x +# ToolchainInfo Version: 2023a +# Specification Revision: 1.0 +# +#------------------------------------------- +# Macros assumed to be defined elsewhere +#------------------------------------------- + +# C_STANDARD_OPTS +# CPP_STANDARD_OPTS +# MINGW_ROOT +# MINGW_C_STANDARD_OPTS + +#----------- +# MACROS +#----------- + +WARN_FLAGS = -Wall -W -Wwrite-strings -Winline -Wstrict-prototypes -Wnested-externs -Wpointer-arith -Wcast-align +WARN_FLAGS_MAX = $(WARN_FLAGS) -Wcast-qual -Wshadow +CPP_WARN_FLAGS = -Wall -W -Wwrite-strings -Winline -Wpointer-arith -Wcast-align +CPP_WARN_FLAGS_MAX = $(CPP_WARN_FLAGS) -Wcast-qual -Wshadow +MW_EXTERNLIB_DIR = $(MATLAB_ROOT)/extern/lib/win64/mingw64 +SHELL = %SystemRoot%/system32/cmd.exe + +TOOLCHAIN_SRCS = +TOOLCHAIN_INCS = +TOOLCHAIN_LIBS = -lws2_32 + +#------------------------ +# BUILD TOOL COMMANDS +#------------------------ + +# C Compiler: GNU C Compiler +CC_PATH = $(MINGW_ROOT) +CC = "$(CC_PATH)/gcc" + +# Linker: GNU Linker +LD_PATH = $(MINGW_ROOT) +LD = "$(LD_PATH)/g++" + +# C++ Compiler: GNU C++ Compiler +CPP_PATH = $(MINGW_ROOT) +CPP = "$(CPP_PATH)/g++" + +# C++ Linker: GNU C++ Linker +CPP_LD_PATH = $(MINGW_ROOT) +CPP_LD = "$(CPP_LD_PATH)/g++" + +# Archiver: GNU Archiver +AR_PATH = $(MINGW_ROOT) +AR = "$(AR_PATH)/ar" + +# MEX Tool: MEX Tool +MEX_PATH = $(MATLAB_ARCH_BIN) +MEX = "$(MEX_PATH)/mex" + +# Download: Download +DOWNLOAD = + +# Execute: Execute +EXECUTE = $(PRODUCT) + +# Builder: GMAKE Utility +MAKE_PATH = $(MINGW_ROOT) +MAKE = "$(MAKE_PATH)/mingw32-make.exe" + + +#------------------------- +# Directives/Utilities +#------------------------- + +CDEBUG = -g +C_OUTPUT_FLAG = -o +LDDEBUG = -g +OUTPUT_FLAG = -o +CPPDEBUG = -g +CPP_OUTPUT_FLAG = -o +CPPLDDEBUG = -g +OUTPUT_FLAG = -o +ARDEBUG = +STATICLIB_OUTPUT_FLAG = +MEX_DEBUG = -g +RM = @del +ECHO = @echo +MV = @move +RUN = + +#-------------------------------------- +# "Faster Runs" Build Configuration +#-------------------------------------- + +ARFLAGS = ruvs +CFLAGS = -c $(MINGW_C_STANDARD_OPTS) -m64 \ + -O3 -fno-loop-optimize -fno-aggressive-loop-optimizations +CPPFLAGS = -c $(CPP_STANDARD_OPTS) -m64 \ + -O3 -fno-loop-optimize -fno-aggressive-loop-optimizations +CPP_LDFLAGS = -static -m64 +CPP_SHAREDLIB_LDFLAGS = -shared -Wl,--no-undefined \ + -Wl,--out-implib,$(notdir $(basename $(PRODUCT))).lib +DOWNLOAD_FLAGS = +EXECUTE_FLAGS = +LDFLAGS = -static -m64 +MEX_CPPFLAGS = +MEX_CPPLDFLAGS = +MEX_CFLAGS = +MEX_LDFLAGS = +MAKE_FLAGS = -f $(MAKEFILE) +SHAREDLIB_LDFLAGS = -shared -Wl,--no-undefined \ + -Wl,--out-implib,$(notdir $(basename $(PRODUCT))).lib + + + +########################################################################### +## OUTPUT INFO +########################################################################### + +PRODUCT = C:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc/mb_fm_mex_source_zermelo.exe +PRODUCT_TYPE = "executable" +BUILD_TYPE = "Executable" + +########################################################################### +## INCLUDE PATHS +########################################################################### + +INCLUDES_BUILDINFO = + +INCLUDES = $(INCLUDES_BUILDINFO) + +########################################################################### +## DEFINES +########################################################################### + +DEFINES_ = -D__USE_MINGW_ANSI_STDIO=1 +DEFINES_CUSTOM = +DEFINES_STANDARD = -DMODEL=mb_fm_mex_source_zermelo + +DEFINES = $(DEFINES_) $(DEFINES_CUSTOM) $(DEFINES_STANDARD) + +########################################################################### +## SOURCE FILES +########################################################################### + +SRCS = $(START_DIR)/mb_fm_mex_source_zermelo.cpp + +ALL_SRCS = $(SRCS) + +########################################################################### +## OBJECTS +########################################################################### + +OBJS = mb_fm_mex_source_zermelo.obj + +ALL_OBJS = $(OBJS) + +########################################################################### +## PREBUILT OBJECT FILES +########################################################################### + +PREBUILT_OBJS = + +########################################################################### +## LIBRARIES +########################################################################### + +LIBS = + +########################################################################### +## SYSTEM LIBRARIES +########################################################################### + +SYSTEM_LIBS = + +########################################################################### +## ADDITIONAL TOOLCHAIN FLAGS +########################################################################### + +#--------------- +# C Compiler +#--------------- + +CFLAGS_BASIC = $(DEFINES) $(INCLUDES) @$(COMPILER_COMMAND_FILE) + +CFLAGS += $(CFLAGS_BASIC) + +#----------------- +# C++ Compiler +#----------------- + +CPPFLAGS_BASIC = $(DEFINES) $(INCLUDES) @$(COMPILER_COMMAND_FILE) + +CPPFLAGS += $(CPPFLAGS_BASIC) + +#--------------------- +# MEX C++ Compiler +#--------------------- + +MEX_CPP_Compiler_BASIC = @$(COMPILER_COMMAND_FILE) + +MEX_CPPFLAGS += $(MEX_CPP_Compiler_BASIC) + +#----------------- +# MEX Compiler +#----------------- + +MEX_Compiler_BASIC = @$(COMPILER_COMMAND_FILE) + +MEX_CFLAGS += $(MEX_Compiler_BASIC) + +########################################################################### +## INLINED COMMANDS +########################################################################### + + +MINGW_C_STANDARD_OPTS = $(C_STANDARD_OPTS) + + +########################################################################### +## PHONY TARGETS +########################################################################### + +.PHONY : all build buildobj clean info prebuild download execute + + +all : build + @echo "### Successfully generated all binary outputs." + + +build : prebuild $(PRODUCT) + + +buildobj : prebuild $(OBJS) $(PREBUILT_OBJS) + @echo "### Successfully generated all binary outputs." + + +prebuild : + + +download : $(PRODUCT) + + +execute : download + @echo "### Invoking postbuild tool "Execute" ..." + $(EXECUTE) $(EXECUTE_FLAGS) + @echo "### Done invoking postbuild tool." + + +########################################################################### +## FINAL TARGET +########################################################################### + +#------------------------------------------- +# Create a standalone executable +#------------------------------------------- + +$(PRODUCT) : $(OBJS) $(PREBUILT_OBJS) + @echo "### Creating standalone executable "$(PRODUCT)" ..." + $(CPP_LD) $(CPP_LDFLAGS) -o $(PRODUCT) @$(CMD_FILE) $(SYSTEM_LIBS) $(TOOLCHAIN_LIBS) + @echo "### Created: $(PRODUCT)" + + +########################################################################### +## INTERMEDIATE TARGETS +########################################################################### + +#--------------------- +# SOURCE-TO-OBJECT +#--------------------- + +%.obj : %.c + $(CC) $(CFLAGS) -o "$@" "$<" + + +%.obj : %.cpp + $(CPP) $(CPPFLAGS) -o "$@" "$<" + + +%.obj : $(RELATIVE_PATH_TO_ANCHOR)/%.c + $(CC) $(CFLAGS) -o "$@" "$<" + + +%.obj : $(RELATIVE_PATH_TO_ANCHOR)/%.cpp + $(CPP) $(CPPFLAGS) -o "$@" "$<" + + +%.obj : $(START_DIR)/%.c + $(CC) $(CFLAGS) -o "$@" "$<" + + +%.obj : $(START_DIR)/%.cpp + $(CPP) $(CPPFLAGS) -o "$@" "$<" + + +%.obj : C:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc/%.c + $(CC) $(CFLAGS) -o "$@" "$<" + + +%.obj : C:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc/%.cpp + $(CPP) $(CPPFLAGS) -o "$@" "$<" + + +mb_fm_mex_source_zermelo.obj : $(START_DIR)/mb_fm_mex_source_zermelo.cpp + $(CPP) $(CPPFLAGS) -o "$@" "$<" + + +########################################################################### +## DEPENDENCIES +########################################################################### + +$(ALL_OBJS) : rtw_proj.tmw $(COMPILER_COMMAND_FILE) $(MAKEFILE) + + +########################################################################### +## MISCELLANEOUS TARGETS +########################################################################### + +info : + @echo "### PRODUCT = $(PRODUCT)" + @echo "### PRODUCT_TYPE = $(PRODUCT_TYPE)" + @echo "### BUILD_TYPE = $(BUILD_TYPE)" + @echo "### INCLUDES = $(INCLUDES)" + @echo "### DEFINES = $(DEFINES)" + @echo "### ALL_SRCS = $(ALL_SRCS)" + @echo "### ALL_OBJS = $(ALL_OBJS)" + @echo "### LIBS = $(LIBS)" + @echo "### MODELREF_LIBS = $(MODELREF_LIBS)" + @echo "### SYSTEM_LIBS = $(SYSTEM_LIBS)" + @echo "### TOOLCHAIN_LIBS = $(TOOLCHAIN_LIBS)" + @echo "### CFLAGS = $(CFLAGS)" + @echo "### LDFLAGS = $(LDFLAGS)" + @echo "### SHAREDLIB_LDFLAGS = $(SHAREDLIB_LDFLAGS)" + @echo "### CPPFLAGS = $(CPPFLAGS)" + @echo "### CPP_LDFLAGS = $(CPP_LDFLAGS)" + @echo "### CPP_SHAREDLIB_LDFLAGS = $(CPP_SHAREDLIB_LDFLAGS)" + @echo "### ARFLAGS = $(ARFLAGS)" + @echo "### MEX_CFLAGS = $(MEX_CFLAGS)" + @echo "### MEX_CPPFLAGS = $(MEX_CPPFLAGS)" + @echo "### MEX_LDFLAGS = $(MEX_LDFLAGS)" + @echo "### MEX_CPPLDFLAGS = $(MEX_CPPLDFLAGS)" + @echo "### DOWNLOAD_FLAGS = $(DOWNLOAD_FLAGS)" + @echo "### EXECUTE_FLAGS = $(EXECUTE_FLAGS)" + @echo "### MAKE_FLAGS = $(MAKE_FLAGS)" + + +clean : + $(ECHO) "### Deleting all derived files ..." + $(RM) $(subst /,\,$(PRODUCT)) + $(RM) $(subst /,\,$(ALL_OBJS)) + $(ECHO) "### Deleted all derived files." + + diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.rsp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.rsp new file mode 100644 index 0000000000..95fd8b72e4 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw.rsp @@ -0,0 +1 @@ +mb_fm_mex_source_zermelo.obj diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw_comp.rsp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw_comp.rsp new file mode 100644 index 0000000000..107d90470e --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw_comp.rsp @@ -0,0 +1,4 @@ +-IC:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc/fm_models/fm_mex_source_zermelo/cpp +-IC:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc +-IC:/Users/xavip/OneDrive/Documentos/GitHub/Swan/OptimalControl/cesc/fm_models/fm_mex_source_zermelo/diff +-IC:/Program\ Files/MATLAB/R2023a/extern/include diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw_ref.rsp b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_rtw_ref.rsp new file mode 100644 index 0000000000..e69de29bb2 diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_types.h b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_types.h new file mode 100644 index 0000000000..743eea9764 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/mb_fm_mex_source_zermelo_types.h @@ -0,0 +1,22 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// mb_fm_mex_source_zermelo_types.h +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +#ifndef MB_FM_MEX_SOURCE_ZERMELO_TYPES_H +#define MB_FM_MEX_SOURCE_ZERMELO_TYPES_H + +// Include files +#include "rtwtypes.h" + +// Custom Header Code +#include +#define CHAR16_T uint16_t +#include "mex.h" +#endif +// End of code generation (mb_fm_mex_source_zermelo_types.h) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/rtw_proj.tmw b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/rtw_proj.tmw new file mode 100644 index 0000000000..fd4b2b7361 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/rtw_proj.tmw @@ -0,0 +1 @@ +Code generation project for mb_fm_mex_source_zermelo using toolchain "MinGW64 | gmake (64-bit Windows)". MATLAB root = C:\Program Files\MATLAB\R2023a. diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/rtwtypes.h b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/rtwtypes.h new file mode 100644 index 0000000000..e537b89b39 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/rtwtypes.h @@ -0,0 +1,44 @@ +// +// Academic License - for use in teaching, academic research, and meeting +// course requirements at degree granting institutions only. Not for +// government, commercial, or other organizational use. +// +// rtwtypes.h +// +// Code generation for function 'mb_fm_mex_source_zermelo' +// + +#ifndef RTWTYPES_H +#define RTWTYPES_H + +/*=======================================================================* + * Fixed width word size data types: * + * int64_T - signed 64 bit integers * + * uint64_T - unsigned 64 bit integers * + *=======================================================================*/ + +#if defined(__APPLE__) +#ifndef INT64_T +#define INT64_T long +#define FMT64 "l" +#if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG) +#define INT_TYPE_64_IS_LONG +#endif +#endif +#endif + +#if defined(__APPLE__) +#ifndef UINT64_T +#define UINT64_T unsigned long +#define FMT64 "l" +#if defined(__LP64__) && !defined(INT_TYPE_64_IS_LONG) +#define INT_TYPE_64_IS_LONG +#endif +#endif +#endif + +// Include files +#include "tmwtypes.h" + +#endif +// End of code generation (rtwtypes.h) diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/setup_mingw.bat b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/setup_mingw.bat new file mode 100644 index 0000000000..4f69e4f2b5 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/cpp/setup_mingw.bat @@ -0,0 +1,2 @@ +set "MINGW_ROOT=C:\PROGRA~3\MATLAB\SUPPOR~1\R2023a\3P778C~1.INS\MINGW_~1.INS\bin" +@set "PATH=%PATH%;%MINGW_ROOT%" \ No newline at end of file diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/mb_fm_mex_source_zermelo.m b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/mb_fm_mex_source_zermelo.m new file mode 100644 index 0000000000..ab68635562 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/mb_fm_mex_source_zermelo.m @@ -0,0 +1,41 @@ +function [ statesdot, j_statesdot ] = mb_fm_mex_source_zermelo(states, controls) +%mb_fm_mex_source_zermelo +% File automatically generated by FALCON.m + +%=== Extract Data From Input ============================================== +x = states(1); +y = states(2); +u = controls(1); + +%=== Jacobians and Hessians =============================================== +j_states = zeros(2, 3); +j_states(:, 1 : 2) = eye(2); +j_x = j_states(1, :); +j_y = j_states(2, :); +j_controls = zeros(1, 3); +j_controls(:, 3 : 3) = eye(1); +j_u = j_controls(1, :); + +%=== Write Constants ====================================================== + +%=== Subsystem source_zermelo ============================================= + +% Defined at ? + +% Call sys_398ef064917340965807c0c11f1a7ead_jac +[ statedottmp, j_statedottmp ] = sys_398ef064917340965807c0c11f1a7ead_jac(states, controls); + +% Hessian Jacobian for sys_398ef064917340965807c0c11f1a7ead_jac +tmp_j_input_sys_398ef064917340965807c0c11f1a7ead_jac = [j_states; j_controls]; + +% Calculation of Jacobian with respect to function global input for sys_398ef064917340965807c0c11f1a7ead_jac +j_statedottmp = j_statedottmp * tmp_j_input_sys_398ef064917340965807c0c11f1a7ead_jac; + +% Combine Variables to statesdot +statesdot = [statedottmp]; +j_statesdot = [ + j_statedottmp(1, :) + j_statedottmp(2, :) +]; + +end diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/sys_398ef064917340965807c0c11f1a7ead-meta.json b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/sys_398ef064917340965807c0c11f1a7ead-meta.json new file mode 100644 index 0000000000..deb927a365 --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/sys_398ef064917340965807c0c11f1a7ead-meta.json @@ -0,0 +1 @@ +{"Class":"falcon.core.builder.DerivativeCacheEntry","Inputs":[{"Rows":2,"Cols":1,"Derivative":true},{"Rows":1,"Cols":1,"Derivative":true}],"Outputs":{"Rows":2,"Cols":1,"Derivative":true,"JacobianSparsity":[[false,true,true],[false,false,true]],"HessianSparsity":[]}} \ No newline at end of file diff --git a/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/sys_398ef064917340965807c0c11f1a7ead_jac.m b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/sys_398ef064917340965807c0c11f1a7ead_jac.m new file mode 100644 index 0000000000..c1fe010c4c --- /dev/null +++ b/OptimalControl/old/cesc/fm_models/fm_mex_source_zermelo/diff/sys_398ef064917340965807c0c11f1a7ead_jac.m @@ -0,0 +1,22 @@ +function [out1,j_out1,h_out1] = sys_398ef064917340965807c0c11f1a7ead_jac(in1,in2val1) +%SYS_398EF064917340965807C0C11F1A7EAD_JAC +% [OUT1,J_OUT1,H_OUT1] = SYS_398EF064917340965807C0C11F1A7EAD_JAC(IN1,IN2VAL1) + +% This function was generated by the Symbolic Math Toolbox version 9.3. +% 27-May-2025 01:35:10 + +%Generated using CreateGradient +%Input dimensions : {[2 1], [1 1]} +%Derivative inputs: [true true] +%DerivativeOrder : 1 +%Output dimensions: {[2 1]} +%Constant outputs : false +in1val2 = in1(2,:); +out1 = [in1val2./2.0+cos(in2val1).*3.0e+1;sin(in2val1).*3.0e+1]; +if nargout > 1 + j_out1 = reshape([0.0,0.0,1.0./2.0,0.0,sin(in2val1).*-3.0e+1,cos(in2val1).*3.0e+1],[2,3]); +end +if nargout > 2 + h_out1 = zeros(0,0); +end +end diff --git a/OptimalControl/old/cesc/main.m b/OptimalControl/old/cesc/main.m new file mode 100644 index 0000000000..6243a7b499 --- /dev/null +++ b/OptimalControl/old/cesc/main.m @@ -0,0 +1,122 @@ +% DIRECT COLLOCATION METHOD => Transcription done by Falcon +% Trapezoidal discretization +% NLP solver -> IPOPT + +clear; +clc; + +falcon.init(); + +%% Define States Controls and Parameter +x_vec = [... + falcon.State('x', -1000, 2000, 0.001);... + falcon.State('y', -1000, 2000, 0.001)]; + +u_vec = [... + falcon.Control('u' , -pi, pi, 1)]; + +tf = falcon.Parameter('FinalTime', 20, 0, 1000, 0.01); + +%% Define Optimal Control Problem +% Create new Problem Instance (Main Instance) +problem = falcon.Problem('Zermelo_wind'); + +% Specify Discretization +tau = linspace(0,1,201); + +% Add a new Phase +phase = problem.addNewPhase(@source_zermelo, x_vec, tau, 0, tf); +phase.addNewControlGrid(u_vec, tau); + + +% Set Boundary Condition +phase.setInitialBoundaries([0,0]); +phase.setFinalBoundaries([1000,200]); + + +% Add Cost Function +problem.addNewLinearPointCost(tf); + + +% Prepare problem for solving +problem.Bake(); + +% Solve problem +solver = falcon.solver.ipopt(problem); +solver.Options.MajorIterLimit = 500; +solver.Options.MajorFeasTol = 1e-6; +solver.Options.MajorOptTol = 1e-6; + +solver.Solve(); + +%% Plots + +results = struct(); +results.t = problem.RealTime; +results.tfinal = problem.Parameters.byName().FinalTime.Value; +results.x = x_vec.extractValuesStructFrom(problem); +results.u = u_vec.extractValuesStructFrom(problem); + +figure(); + +ax = subplot(2, 2, 1); +plot(ax, results.t, rad2deg(results.u.u)); +xlabel(ax, "t"); +ylabel(ax, "alfa (deg)"); + +ax = subplot(2, 2, 2); +plot(ax, results.x.x, results.x.y); +xlabel(ax, "x"); +ylabel(ax, "y"); + +% Plot with wind vector field + +% Define the range for x and y +x = linspace(0, 1100, 20); +y = linspace(0, 250, 20); + +% Create meshgrid for x and y +[X, Y] = meshgrid(x, y); + +% Define the vector field +vx = 0.05 * Y; +vy= 0 * X; + +set(groot, 'defaultTextInterpreter', 'latex'); +set(groot, 'defaultAxesTickLabelInterpreter', 'latex'); +set(groot, 'defaultLegendInterpreter', 'latex'); + +% Plot the vector field and the trajectory using quiver +figure; +quiver(X, Y, vx, vy, 'AutoScaleFactor', 0.75); +hold on; % Keep the figure for further plotting + +% Plot the trajectory on top of the vector field +plot(results.x.x, results.x.y, 'b', 'LineWidth', 2); + +% Plot starting point +plot(results.x.x(1), results.x.y(1), 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); + +% Plot target point +plot(results.x.x(end), results.x.y(end), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); + +% Add annotations and grid +xlim([0 1000]); % Setting x-axis limits +ylim([0 250]); % Setting y-axis limits +title('Wind Vector Field with Trajectory'); +xlabel('X Coordinate'); +ylabel('Y Coordinate'); +legend('Wind Vectors', 'Trajectory', 'Location', 'best'); +grid on; +hold off; % Release the figure + +% Control plot + +figure; +plot(results.t, rad2deg(results.u.u), 'r', 'LineWidth', 2); +grid on; +xlabel('Time [s]'); +ylabel('\alpha [^\circ]'); +title('Control Angle \alpha Over Time'); + +results.tfinal diff --git a/OptimalControl/old/cesc/source_zermelo.m b/OptimalControl/old/cesc/source_zermelo.m new file mode 100644 index 0000000000..e2f52316ff --- /dev/null +++ b/OptimalControl/old/cesc/source_zermelo.m @@ -0,0 +1,24 @@ +function [states_dot] = source_zermelo(states, controls) %#codegen + +% model interface created by falcon.m + +% Extract states +x = states(1); +y = states(2); + +% Extract controls +u = controls(1); + +% Constants +V=30; % m/s + +% ------------------------ % +% implement the model here % +% ------------------------ % + +% implement state derivatives here +x_dot = V*cos(u) + 0.5*y; +y_dot = V*sin(u); +states_dot = [x_dot; y_dot]; +% EoF +end \ No newline at end of file diff --git a/OptimalControl/old/drag pruebas/drag_data.mat b/OptimalControl/old/drag pruebas/drag_data.mat new file mode 100644 index 0000000000..cb06a5c239 Binary files /dev/null and b/OptimalControl/old/drag pruebas/drag_data.mat differ diff --git a/OptimalControl/old/drag pruebas/main_constraint_drag.m b/OptimalControl/old/drag pruebas/main_constraint_drag.m new file mode 100644 index 0000000000..0cc25971d8 --- /dev/null +++ b/OptimalControl/old/drag pruebas/main_constraint_drag.m @@ -0,0 +1,212 @@ +function main_constraint_drag + persistent u_history J_history; + clc; close all + + g = 9.81; % Gravity [m/s^2] + rho = 1.225; % Air density [kg/m^3] + Sw = 2; % Wing/Aerodynamic surface [m^2] + Cd0 = 0.01; % Drag coefficient + m = 1; % Object mass [kg] + k = 0.05; + Clalpha = 2*pi; + Cl0 = 0; + + N = 50; % Discretization + + x1_0 = 0; x2_0 = 0; v0 = 15; + gamma0 = deg2rad(40); + t0 = 0; tf = 30; + alpha0 = deg2rad(10); + u0 = [tf; alpha0]; + lb = [0 0.01]; % Lower bounds for the control + ub = [500 deg2rad(10)]; % Upper bounds for the control + + u0 = [u0(1) ones(1,N)*u0(2)]; + lb = [lb(1) ones(1,N)*lb(2)]; + ub = [ub(1) ones(1,N)*ub(2)]; + + % Previous calculations (Derivatives) + + Cl = @(alpha) Cl0 + Clalpha*alpha; + Cd = @(alpha) Cd0 + k*Cl(alpha).^2; + D = @(V,alpha) 0.5*rho*Sw*V.^2.*Cd(alpha); + + dCl = Clalpha; % Derivative respecto to alpha + dCd = @(alpha) 2*k*Cl(alpha)*dCl; % Derivative respect to alpha + dDda = @(V,alpha) 0.5*rho*Sw*V.^2.*dCd(alpha); % Derivative respect to alpha + + dDdv = @(V,alpha) rho*Sw*V.*Cd(alpha); % Derivative respect to velocity + + y0 = [x1_0 x2_0 v0 gamma0]; + cost = @(u) f_cost(u, g, t0, y0, D, dDda, dDdv, N, m); + constraint = @(u) groundConstraint(u, g, t0, v0, x1_0, x2_0, gamma0, D, dDda, dDdv, N, m); + +% [valid,err] = checkGradients(cost, u0, 'Tolerance', 1e-3, 'Display', 'on'); +% assignin('base', 'err', err); + %[valid2,err2] = checkGradients(constraint, u0, 'Tolerance', 1e-3, 'Display', 'on') + %assignin('base', 'err2', err2); + + options = optimoptions("fmincon", ... + "OutputFcn", @store_fmincon, ... + "Algorithm", "sqp", ... + "DerivativeCheck", "on","Display","iter"); + + [u_opt, ~] = fmincon(cost, u0, [], [], [], [], lb, ub, @(u) nonlcon(u,constraint), options); + + y0 = [x1_0 x2_0 v0 gamma0]; + t_span = linspace(t0, u_opt(1), N); + alpha = u_opt(2:N+1); + f = @(t,y,alpha) dynamics(t, y, alpha, g, D, m); + [~, y] = trapezoidal(f, t_span, y0,alpha); + + disp(["Maximum distance [m] = ", num2str(y(end, 1))]) + disp(["Initial angle [°]: ", num2str(rad2deg(u_opt(2)))]) + disp(["Final Time [s]: ", num2str(u_opt(1))]) + + figure; plot(y(:,1), y(:,2), 'b-', 'LineWidth', 2); + xlabel("Horizontal distance [m]"); ylabel("Vertical distance [m]"); grid on; + + figure; plot(u_history(:,1), 'b-o', 'LineWidth', 2); + xlabel('Iteration'); ylabel('Final time [s]'); grid on; + + figure; plot(rad2deg(u_history(:,2)), 'r-o', 'LineWidth', 2); + xlabel('Iteration'); ylabel('Initial angle [°]'); grid on; + + figure; plot(J_history, 'o-', 'LineWidth', 2); + xlabel('Iteration'); ylabel('Cost function J'); grid on; + + vars = whos; + nodrag_results = struct(); + for i = 1:length(vars) + nodrag_results.(vars(i).name) = eval(vars(i).name); + end + + function stop = store_fmincon(u, optimValues, state) + if optimValues.iteration == 0 + u_history = []; + J_history = []; + end + if strcmp(state, 'iter') + u_history = [u_history; u(:)']; + J_history = [J_history; optimValues.fval]; + assignin('base', 'u_iterations', u_history); + assignin('base', 'J_iterations', J_history); + end + stop = false; + end +end + +function [c, ceq, Dc, Dceq] = nonlcon(u,constraint) + c = []; + Dc = []; + [ceq, Dceq] = constraint(u); +end + +function [J, gradJ] = f_cost(u, g, t0, y0, D, dDdv, dDda, N, m) + tf = u(1); + alpha = u(2:N+1); + t_span = linspace(t0, tf, N); + f = @(t,y,alpha) dynamics(t, y, alpha, g, D, m); + [~, y] = trapezoidal(f, t_span, y0,alpha); + + J = -y(end,1); + dydt_final = dynamics(tf, y(end,:), alpha(end), g, D, m); + + pT = [1 0 0 0]; + dp = @(t,p,alpha) adjoint(t, p, y, g, alpha, dDdv, m); + [~, p] = trapezoidal(dp, flip(t_span), pT,alpha); + + % ind = round((t - t_span(1)) / (t_span(2) - t_span(1))) + 1; + % v = y(ind,3); + %v = interp1(t_span, y(:,3), t); + v = y(:,3); + DFdu = -dDda(v,alpha')./m; + % gradJ = [-dydt_final(1); DFdu.*p(:,3)]; + gradJ = zeros(N+1,1); + gradJ(1) = -dydt_final(1); + for i = 1:N + gradJ(i+1) = DFdu(i) * p(i,3); + end +end + +function [t_span,y] = trapezoidal(f,t_span, y0, alpha) + N = length(t_span); + dt = (t_span(end)-t_span(1)) / (N-1); + y = zeros(4,N); + y(:,1) = y0; + for i = 1:N-1 + ti = t_span(i); + ti1 = t_span(i+1); + alphai = alpha(i); + alphai1 = alpha(i+1); + yi = y(:,i); + fi = f(ti, yi,alphai); + y_pred = yi + dt*fi; + fi1 = f(ti1, y_pred, alphai1); + y(:,i+1) = yi + (dt/2)*(fi + fi1); + end + y = y'; +end + + +function [ceq, Dceq] = groundConstraint(u, g, t0, v0, x1_0, x2_0, gamma0, D, dDdv, dDda, N, m) + y0 = [x1_0 x2_0 v0 gamma0]'; + tf = u(1); + t_span = linspace(t0, tf, N); + alpha = u(2:N+1); + [~, y] = trapezoidal(@(t, y,alpha) dynamics(t, y,alpha, g, D, m), t_span, y0,alpha); + + ceq = y(end,2); + dydt_final = dynamics(t_span(end), y(end,:), alpha(end), g, D, m); + + pT = [0 -1 0 0]; + dp = @(t,p,alpha) adjoint(t, p, y, g, alpha, dDdv, m); + [~, p] = trapezoidal(dp, flip(t_span), pT,alpha); + + % v = interp1(t_span, y(:,3), t); + % ind = round((t - t_span(1)) / (t_span(2) - t_span(1))) + 1; + % v = y(ind,3); + v = y(:,3); + DFdu = -dDda(v,alpha'); + + Dceq = [dydt_final(2); DFdu.*p(:,3)]; + c = []; + dc = []; +end + +function dpdt = adjoint(t, p, y, g, alpha, dDdv, m) + v = y(3); + gamma = y(4); + % v = interp1(t_span, y(:,3), t); + % gamma = interp1(t_span, y(:,4), t); + % alpha = interp1(t_span, alpha, t); + + % ind = round((t - t_span(1)) / (t_span(2) - t_span(1))) + 1; + % v = y(ind,3); + % gamma = y(ind,4); + % alpha = alpha(ind); + + J = [0 0 0 0; + 0 0 0 0; + cos(gamma) sin(gamma) -dDdv(v,alpha)/m (g/v^2)*cos(gamma); + -v*sin(gamma) v*cos(gamma) -g*cos(gamma) (g/v)*sin(gamma)]; + dpdt = -J*p; +end + + +function dydt = dynamics(t, y, alpha, g, D, m) + v = y(3); + gamma = y(4); + + % t_span = linspace(0, tf, N); + % ind = round((t - t_span(1)) / (t_span(2) - t_span(1))) + 1; + % alpha = alpha(ind); + + % alpha = interp1(t_span, alpha, t); + + dydt = [v*cos(gamma); + v*sin(gamma); + -g*sin(gamma)-D(v,alpha)/m; + -(g/v)*cos(gamma) + ]; +end diff --git a/OptimalControl/old/drag pruebas/nodrag_data.mat b/OptimalControl/old/drag pruebas/nodrag_data.mat new file mode 100644 index 0000000000..0bb8bd9ca7 Binary files /dev/null and b/OptimalControl/old/drag pruebas/nodrag_data.mat differ diff --git a/OptimalControl/old/drag y lift/main_constraint_draglift.m b/OptimalControl/old/drag y lift/main_constraint_draglift.m new file mode 100644 index 0000000000..a0298f008a --- /dev/null +++ b/OptimalControl/old/drag y lift/main_constraint_draglift.m @@ -0,0 +1,149 @@ +function main_constraint_draglift + persistent u_history J_history; + clc; close all + + data = load('resultsfalcon.mat'); + resultsfalcon = data.results; + clear data + g = 9.81; % Gravity [m/s^2] + rho = 1.225; % Air density [kg/m^3] + Sw = 0.6; % Wing/Aerodynamic surface [m^2] + Cd0 = 0.03; % Drag coefficient + m = 5; % Object mass [kg] + k = 0.05; + Clalpha = 0.1*360/(2*pi); % en radianes + %Clalpha = 0.1; % en grados + Cl0 = 0; + + N = 50; % Discretization + + x1_0 = 0; x2_0 = 0; v0 = 15; + gamma0 = deg2rad(30); + t0 = 0; tf_guess = 3; + alpha0 = deg2rad(3); + tf_falcon = resultsfalcon.tfinal; + t_span_falcon = linspace(0,tf_falcon,250); + t_span = linspace(0,tf_falcon,N); + alpha_falcon = interp1(t_span_falcon,resultsfalcon.u.alpha,t_span); + + %u0 = [tf_falcon+rand(1,1)*0*norm(tf_falcon) alpha_falcon+rand(1,1)*0*norm(alpha_falcon)]; + %u0 = [tf_guess alpha0*ones(1,N)]; + u0 = [tf_falcon alpha_falcon]; + lb = [0.1 deg2rad(-10)]; % Lower bounds for the control + ub = [15 deg2rad(15)]; % Upper bounds for the control + + %u0 = [u0(1) ones(1,N)*u0(2)]; + lb = [lb(1) ones(1,N)*lb(2)]; + ub = [ub(1) ones(1,N)*ub(2)]; + + % Previous calculations (Derivatives) + + Cl = @(alpha) Cl0 + Clalpha*alpha; + Cd = @(alpha) Cd0 + k*Cl(alpha).^2; + D = @(V,alpha) 0.5*rho*Sw*V.^2.*Cd(alpha); + L = @(V,alpha) 0.5*rho*Sw*V.^2.*Cl(alpha); + + cost = @(u) f_cost(u, g, t0, v0, gamma0, x1_0, x2_0, D, N, m, L); + nonlcon_fun = @(u) nonlcon_full(u, g, t0, v0, x1_0, x2_0, gamma0, D, N, m, L); + + options = optimoptions('fmincon', ... + 'Algorithm', 'sqp', ... + 'TolX', 1e-8, ... % precisión en las variables + 'TolFun', 1e-8, ... % precisión en el valor de la función objetivo + 'TolCon', 1e-6, ... % precisión en las restricciones + 'MaxIter', 500, ... % aumentar número de iteraciones + 'MaxFunEvals', 5000, ... % aumentar evaluaciónes de la función + 'Display', 'iter', ... + 'FiniteDifferenceType', 'central', ... + 'FiniteDifferenceStepSize', 1e-6); + + [u_opt, ~] = fmincon(cost, u0, [], [], [], [], lb, ub, nonlcon_fun, options); + + y0 = [x1_0 x2_0 v0 gamma0]; + tf = u_opt(1); + t_span = linspace(t0, tf, N); + [~, y] = ode45(@(t, y) dynamics(t, y, tf, u_opt(2:N+1), g, D, N, m, L), t_span, y0); + + disp(["Maximum distance [m] = ", num2str(y(end, 1))]) + disp(["Final Time [s]: ", num2str(u_opt(1))]) + + figure + plot(y(:,1),y(:,2)) + xlabel("Horizontal displacement [m]") + ylabel("Vertical displacement [m]") + title("Trajectory") + + function stop = store_fmincon(u, optimValues, state) + if optimValues.iteration == 0 + u_history = []; + J_history = []; + end + if strcmp(state, 'iter') + u_history = [u_history; u(:)']; + J_history = [J_history; optimValues.fval]; + assignin('base', 'u_iterations', u_history); + assignin('base', 'J_iterations', J_history); + end + stop = false; + end + +end + +function [J] = f_cost(u, g, t0, v0, gamma0, x1_0, x2_0, D, N, m, L) + y0 = [x1_0 x2_0 v0 gamma0]; + tf = u(1); + alpha = u(2:N+1); + t_span = linspace(t0, tf, N)'; + ode_opts = odeset('RelTol',1e-5,'AbsTol',1e-7); + [~, y] = ode45(@(t, y) dynamics(t, y, tf, alpha, g, D, N, m, L), t_span, y0, ode_opts); + + J = -y(end,1); + + % figure(100); plot(y(:,1), y(:,2), 'b-+', 'LineWidth', 2); + % xlabel("Horizontal distance [m]"); ylabel("Vertical distance [m]"); grid on; + % xlim([0 200]) + % ylim([-50 50]) + % + % figure(101); plot(t_span, rad2deg(u(2:end)), 'b-+', 'LineWidth', 2); + % xlabel("Time [t]"); ylabel("angle of attack [deg]"); grid on; + % xlim([0 tf]) + % ylim([0 10]) + % + % figure(102); plot(t_span, (y(:,3)), 'b-+', 'LineWidth', 2); + % xlabel("Time [t]"); ylabel("Velocity [m/s]"); grid on; + % xlim([0 tf]) + % ylim([-50 50]) + % + % figure(103); plot(J); + % hold on + +end + +function [c, ceq] = nonlcon_full(u, g, t0, v0, gamma0, x1_0, x2_0, D, N, m, L) + tf = u(1); + alpha = u(2:end); + t_span = linspace(t0, tf, N); + y0 = [x1_0; x2_0; v0; gamma0]; + ode_opts = odeset('RelTol',1e-5,'AbsTol',1e-7); + [~, y] = ode45(@(t,y)dynamics(t,y,tf,alpha,g,D,N,m,L), t_span, y0, ode_opts); + + minY = min(y(:,2)); + c = -minY; % cumple si minY ≥ 0 + ceq = y(end,2); + +end + +function dydt = dynamics(t, y, tf, alpha, g, D, N, m, L) + v = y(3); + gamma = y(4); + + t_span = linspace(0, tf, N); + alpha = interp1(t_span, alpha, t); + + dydt = [ + v*cos(gamma); + v*sin(gamma); + -g*sin(gamma)-D(v,alpha)/m; + -(g/v)*cos(gamma)+L(v,alpha)/(m*v); + ]; +end diff --git a/OptimalControl/old/drag y lift/opt_tiro_sens.m b/OptimalControl/old/drag y lift/opt_tiro_sens.m new file mode 100644 index 0000000000..9ad14ce746 --- /dev/null +++ b/OptimalControl/old/drag y lift/opt_tiro_sens.m @@ -0,0 +1,223 @@ +function opt_tiro_sens +%------------------------------------------------------------- +% ÓPTIMO DE TIRO PARABÓLICO CON DRAG & LIFT +% Derivadas analíticas (sensibilidades variacionales) +%------------------------------------------------------------- +clc; close all; + +%% ------------------ PARÁMETROS FÍSICOS ------------------- +pars.g = 9.81; % gravedad [m/s^2] +pars.rho = 1.225; % densidad aire [kg/m^3] +pars.Sw = 0.6; % superficie alar [m^2] +pars.Cd0 = 0.03; +pars.k = 0.05; +pars.m = 5; % masa [kg] +pars.Cl0 = 0; +pars.Clalpha = 0.1*360/(2*pi); % [1/rad] (0.1 por grado) + +%% ------------------ DISCRETIZACIÓN ------------------------- +N = 200; % nº nodos de alfa(t) +pars.N = N; +pars.s_nodes = linspace(0,1,N); % malla normalizada 0…1 +nu = N + 1; % nº variables de decisión + +%% ------------------ CONDICIONES INICIALES ----------------- +x1_0 = 0; x2_0 = 0; v0 = 15; +gamma0 = deg2rad(30); % ángulo de disparo +y0 = [x1_0; x2_0; v0; gamma0]; + +%% ------------------ VARIABLES DE DECISIÓN ----------------- +tf0 = 10; % conjetura tf +alpha0 = deg2rad(4); % conjetura α(t) +u0 = [ tf0, ones(1,N)*alpha0 ]; + +lb = [ 0.1, ones(1,N)*deg2rad(-10) ]; +ub = [ 15 , ones(1,N)*deg2rad( 10) ]; +u0 = max(lb, min(ub,u0)); % clip → dentro de límites + +%% ------------------ OPCIONES FMINCON ---------------------- +options = optimoptions('fmincon', ... + 'Algorithm', 'sqp', ... + 'SpecifyObjectiveGradient', true, ... + 'SpecifyConstraintGradient', true, ... + 'UseParallel', true, ... + 'FiniteDifferenceType', 'central', ... % sólo por si falláramos + 'Display', 'iter'); + +%% ------------------ OPTIMIZACIÓN -------------------------- +costfun = @(u) cost_with_grad (u,y0,pars); +confun = @(u) con_with_grad (u,y0,pars); + +[u_opt,~,exitflag,output] = ... + fmincon(costfun,u0,[],[],[],[],lb,ub,confun,options); + +%% ------------------ RESULTADOS ---------------------------- +fprintf('\n· Alcance máximo = %.3f m\n', -costfun(u_opt)); +fprintf('· Tiempo de vuelo = %.3f s\n', u_opt(1)); +fprintf('· Iteraciones = %d\n', output.iterations); +fprintf('· Mensaje solver : %s\n\n', output.message); + +% Trayectoria óptima para visualizar +[Y,~] = simulate_state_sens(u_opt,y0,pars); +figure; plot(Y(:,1),Y(:,2),'LineWidth',1.6); +xlabel('Distancia horizontal x [m]'); +ylabel('Altura y [m]'); grid on; title('Trayectoria óptima'); +end +%% ===================================================================== +% COSTE + GRADIENTE +% ====================================================================== +function [J, gradJ] = cost_with_grad(u,y0,pars) +[Y,S] = simulate_state_sens(u,y0,pars); + +J = -Y(end,1); % max alcance → min ( -x(tf) ) +gradJ = -S(end,1,:); % d(-x)/du = - ∂x/∂u +gradJ = gradJ(:); % columna +end +%% ===================================================================== +% RESTRICCIONES + GRADIENTES ANALÍTICOS +% ====================================================================== +function [c,ceq,gradc,gradceq] = con_with_grad(u,y0,pars) +[Y,S] = simulate_state_sens(u,y0,pars); + +% --- Desigualdades: y(t) >= 0 → -y <= 0 +c = -Y(:,2); % tamaño N×1 +St = squeeze(S(:,2,:)); % (N × nu) +gradc = -St.'; % (nu × N) + +% --- Igualdad final: y(tf) = 0 +ceq = Y(end,2); % escalar +gradceq = squeeze(S(end,2,:)); % (nu × 1) +end +%% ===================================================================== +% SIMULACIÓN DEL SISTEMA + SENSIBILIDADES (con cache) +% ====================================================================== +function [Y,S] = simulate_state_sens(u,y0,pars) +% Devuelve: +% Y : (N × 4) estados en los nodos +% S : (N × 4 × nu) sensibilidades +persistent u_last Y_last S_last +if ~isempty(u_last) && isequal(u,u_last) + Y = Y_last; S = S_last; return % reutiliza integración +end + +tf = u(1); +alpha_n = u(2:end); % N valores +nu = numel(u); + +z0 = zeros(4 + 4*nu,1); % estado + sens. +z0(1:4) = y0; + +t_span = linspace(0,tf,pars.N); % nodos reales (N tiempos) + +odeopts = odeset('RelTol',1e-6,'AbsTol',1e-8); + +odefun = @(t,z) aug_dynamics(t,z,tf,alpha_n,pars); + +[~,Z] = ode45(odefun,t_span,z0,odeopts); + +Y = Z(:,1:4); % N × 4 +Sens = Z(:,5:end); % N × 4·nu +S = reshape(Sens,[],4,nu); % N × 4 × nu + +% Guarda en cache +u_last = u; Y_last = Y; S_last = S; +end +%% ===================================================================== +% DINÁMICA AUMENTADA (estado+sens.) +% ====================================================================== +function dzdt = aug_dynamics(t,z,tf,alpha_n,pars) +% --- desempaqueta -------------------------------------------- +N = pars.N; +nu = N + 1; +y = z(1:4); % estado +S = reshape(z(5:end),4,nu); % 4×nu sensibilidades + +x = y(1); y_pos = y(2); %#ok +v = y(3); gamma = y(4); + +% --- α(t), dα/dtf y pesos dα/dα_k ----------------------------- +[alpha, w, dalpha_dtf] = alpha_info(t,tf,alpha_n,pars); + +% ---- COEFICIENTES AERODINÁMICOS ------------------------------ +g = pars.g; rho = pars.rho; Sw = pars.Sw; +m = pars.m; Cd0 = pars.Cd0; k = pars.k; +Cl0 = pars.Cl0; Clalpha = pars.Clalpha; + +Cl = Cl0 + Clalpha*alpha; +Cd = Cd0 + k*Cl.^2; + +D = 0.5*rho*Sw*v^2*Cd; +L = 0.5*rho*Sw*v^2*Cl; + +% --- DINÁMICA f(y,α) ------------------------------------------ +f = [ ... + v*cos(gamma); ... + v*sin(gamma); ... + -g*sin(gamma) - D/m; ... + -(g/v)*cos(gamma) + L/(m*v) ]; + +%% --------------- MATRIZ A = ∂f/∂y ------------------------- +A = zeros(4,4); +A(1,3) = cos(gamma); +A(1,4) = -v*sin(gamma); + +A(2,3) = sin(gamma); +A(2,4) = v*cos(gamma); + +c1 = 0.5*rho*Sw*Cd; +A(3,3) = -(1/m) * 2*c1*v; +A(3,4) = -g*cos(gamma); + +k2 = (0.5*rho*Sw*Cl)/m; +A(4,3) = g*cos(gamma)/v^2 + k2; +A(4,4) = (g/v)*sin(gamma); + +%% -------- B = ∂f/∂u_j (α_k y t_f) ------------------------- +% d f / d α +dCd_dalp = 2*k*Cl*Clalpha; +dD_dalp = 0.5*rho*Sw*v^2*dCd_dalp; +dL_dalp = 0.5*rho*Sw*v^2*Clalpha; + +df_dalp = [ 0; 0; + -(1/m)*dD_dalp; + (1/(m*v))*dL_dalp ]; + +B = zeros(4,nu); +% primera columna → t_f +B(:,1) = df_dalp * dalpha_dtf; % 4×1 +% columnas 2..N+1 → α_k +B(:,2:end) = df_dalp .* w; % 4×N (broadcast) + +%% ------------ Ecuación de sensibilidades -------------------- +dSdt = A*S + B; % 4×nu + +%% ------------ Empaqueta derivadas --------------------------- +dzdt = [ f ; dSdt(:) ]; +end +%% ===================================================================== +% α(t), pesos dα/dα_k y derivada dα/dt_f (malla s = t/tf) +% ====================================================================== +function [alpha, w, dalpha_dtf] = alpha_info(t,tf,alpha_n,pars) +N = pars.N; +s = min(max(t/tf,0),1); % s ∈ [0,1] +% nodos uniformes en s +i = floor(s*(N-1))+1; % índice base (1…N) +if i>=N + i = N-1; lambda = 1; % extremo derecho +elseif i<=0 + i = 1; lambda = 0; % extremo izquierdo +else + s_i = (i-1)/(N-1); + lambda = (s - s_i) * (N-1); % ∈ [0,1] +end +% pesos lineales +w = zeros(1,N); +w(i) = 1 - lambda; +w(i+1) = lambda; + +alpha = w*alpha_n.'; % α(t) + +% ←—— derivada respecto tf ——————————————— +dalpha_ds = (N-1)*(alpha_n(i+1) - alpha_n(i)); % pendiente local +dalpha_dtf = - (s/tf) * dalpha_ds; % dα/dtf +end diff --git a/OptimalControl/old/drag y lift/resultsfalcon.mat b/OptimalControl/old/drag y lift/resultsfalcon.mat new file mode 100644 index 0000000000..fcc606ddc5 Binary files /dev/null and b/OptimalControl/old/drag y lift/resultsfalcon.mat differ diff --git a/OptimalControl/old/drag/drag_data.mat b/OptimalControl/old/drag/drag_data.mat new file mode 100644 index 0000000000..cb06a5c239 Binary files /dev/null and b/OptimalControl/old/drag/drag_data.mat differ diff --git a/OptimalControl/old/drag/main_constraint_drag.m b/OptimalControl/old/drag/main_constraint_drag.m new file mode 100644 index 0000000000..5934b6fdf7 --- /dev/null +++ b/OptimalControl/old/drag/main_constraint_drag.m @@ -0,0 +1,260 @@ +function main_constraint_drag + persistent u_history J_history; + clc; close all + + g = 9.81; % Gravity [m/s^2] + rho = 1.225; % Air density [kg/m^3] + Sw = 0.6; % Wing/Aerodynamic surface [m^2] + Cd0 = 0.03; % Drag coefficient + m = 5; % Object mass [kg] + k = 0.05; + Clalpha = 5.7296; + Cl0 = 0; + + N = 50; % Discretization + + x1_0 = 0; x2_0 = 0; v0 = 15; + gamma0 = deg2rad(40); + t0 = 0; tf = 3; + alpha0 = deg2rad(3); + u0 = [tf; alpha0]; + lb = [0 0]; % Lower bounds for the control + ub = [5 deg2rad(10)]; % Upper bounds for the control + + u0 = [u0(1) ones(1,N)*u0(2)]; + lb = [lb(1) ones(1,N)*lb(2)]; + ub = [ub(1) ones(1,N)*ub(2)]; + + % Previous calculations (Derivatives) + + Cl = @(alpha) Cl0 + Clalpha*alpha; + Cd = @(alpha) Cd0 + k*Cl(alpha).^2; + D = @(V,alpha) 0.5*rho*Sw*V.^2.*Cd(alpha); + + dCl = Clalpha; % Derivative respecto to alpha + dCd = @(alpha) 2*k*Cl(alpha)*dCl; % Derivative respect to alpha + dDda = @(V,alpha) 0.5*rho*Sw*V.^2.*dCd(alpha); % Derivative respect to alpha + + dDdv = @(V,alpha) rho*Sw*V.*Cd(alpha); % Derivative respect to velocity + + cost = @(u) f_cost(u, g, t0, v0, gamma0, x1_0, x2_0, D, dDda, dDdv, N, m); + constraint = @(u) groundConstraint(u, g, t0, v0, x1_0, x2_0, gamma0, D, dDda, dDdv, N, m); + % + % eps = 1e-6; + % [cost0, grad0] = cost(u0); + % for i=1:N+1 + % uI = u0; + % uI(i) = uI(i) + eps*u0(i); + % costI(i) = cost(uI); + % gradD(i) = (costI(i)-cost0)/(uI(i)-u0(i)); + % end + + % plot(grad0(1:end),'-+') + % hold on + % plot(gradD(1:end),'-+') + + % Coste centrada + + % eps = 1e-6; + % [cost0, grad0] = cost(u0); + % + % gradD = zeros(N+1, 1); % inicializar vector de derivadas + % + % for i = 1:N+1 + % u_forward = u0; + % u_backward = u0; + % + % % Asegura que el paso no sea nulo si u0(i) es cero + % delta = eps * max(1, abs(u0(i))); + % + % u_forward(i) = u_forward(i) + delta*u0(i); + % u_backward(i) = u_backward(i) - delta*u0(i); + % + % cost_forward = cost(u_forward); + % cost_backward = cost(u_backward); + % + % % Diferencia centrada + % gradD(i) = (cost_forward - cost_backward) / (2 * delta); + % end + % + % plot(grad0(2:end)/(tf/N), '-+') + % hold on + % plot(gradD(2:end), '-+') + % legend('Gradiente analítico', 'Gradiente centrado numérico') + % xlabel('Índice de variable de control') + % ylabel('Derivada') + % title('Comparación entre gradiente del coste analítico y numérico centrado') + + + % eps = 1e-6; + % [constraint0,gradconst0] = constraint(u0); + % for i=1:N+1 + % uI = u0; + % uI(i) = uI(i) + eps*u0(i); + % constraintI(i) = constraint(uI); + % gradconstD(i) = (constraintI(i)-constraint0)/(uI(i)-u0(i)); + % end + % + % figure + % plot(gradconst0/(tf/N),'-+') + % hold on + % plot(gradconstD,'-+') + + % Centrada + + % eps = 1e-6; + % [constraint0, gradconst0] = constraint(u0); + % + % gradconstD = zeros(N+1, 1); % inicializar vector de derivadas + % + % for i = 1:N+1 + % u_forward = u0; + % u_backward = u0; + % + % % evitar que eps*u0(i) sea cero si u0(i) lo es + % delta = eps * max(1, abs(u0(i))); + % + % u_forward(i) = u_forward(i) + delta; + % u_backward(i) = u_backward(i) - delta; + % + % constraint_forward = constraint(u_forward); + % constraint_backward = constraint(u_backward); + % + % % derivada centrada + % gradconstD(i) = (constraint_forward - constraint_backward) / (2 * delta); + % end + % + % figure + % plot(gradconst0(2:end)/(tf/N), '-+') + % hold on + % plot(gradconstD(2:end), '-+') + % legend('Gradiente analítico', 'Gradiente centrado numérico') + % xlabel('Índice de variable de control') + % ylabel('Derivada') + % title('Comparación entre gradiente analítico y numérico centrado') + % + % [valid,err] = checkGradients(cost, u0, 'Tolerance', 1e-3, 'Display', 'on'); + % assignin('base', 'err', err); + % [valid2,err2] = checkGradients(constraint, u0, 'Tolerance', 1e-3, 'Display', 'on') + % assignin('base', 'err2', err2); + + + options = optimoptions("fmincon", ... + "OutputFcn", @store_fmincon, ... + "Algorithm", "sqp", ... + "DerivativeCheck", "on","Display","iter"); + + [u_opt, ~] = fmincon(cost, u0, [], [], [], [], lb, ub, @(u) nonlcon(u,constraint), options); + + y0 = [x1_0 x2_0 v0 gamma0]; + t_span = linspace(t0, u_opt(1), N); + [~, y] = ode45(@(t, y) dynamics(t, y, tf, u_opt(2:N+1), g, D, N, m), t_span, y0); + + disp(["Maximum distance [m] = ", num2str(y(end, 1))]) + disp(["Initial angle [°]: ", num2str(rad2deg(u_opt(2)))]) + disp(["Final Time [s]: ", num2str(u_opt(1))]) + + figure; plot(y(:,1), y(:,2), 'b-', 'LineWidth', 2); + xlabel("Horizontal distance [m]"); ylabel("Vertical distance [m]"); grid on; + + figure; plot(u_history(:,1), 'b-o', 'LineWidth', 2); + xlabel('Iteration'); ylabel('Final time [s]'); grid on; + + figure; plot(rad2deg(u_history(:,2)), 'r-o', 'LineWidth', 2); + xlabel('Iteration'); ylabel('Initial angle [°]'); grid on; + + figure; plot(J_history, 'o-', 'LineWidth', 2); + xlabel('Iteration'); ylabel('Cost function J'); grid on; + + vars = whos; + nodrag_results = struct(); + for i = 1:length(vars) + nodrag_results.(vars(i).name) = eval(vars(i).name); + end + + function stop = store_fmincon(u, optimValues, state) + if optimValues.iteration == 0 + u_history = []; + J_history = []; + end + if strcmp(state, 'iter') + u_history = [u_history; u(:)']; + J_history = [J_history; optimValues.fval]; + assignin('base', 'u_iterations', u_history); + assignin('base', 'J_iterations', J_history); + end + stop = false; + end +end + +function [c, ceq] = nonlcon(u,constraint) + c = []; + Dc = []; + [ceq] = constraint(u); +end + +function [J] = f_cost(u, g, t0, v0, gamma0, x1_0, x2_0, D, dDdv, dDda, N, m) + y0 = [x1_0 x2_0 v0 gamma0]; + tf = u(1); + alpha = u(2:N+1); + t_span = linspace(t0, tf, N)'; + [~, y] = ode45(@(t, y) dynamics(t, y, tf, alpha, g, D, N, m), t_span, y0); + + J = -y(end,1); + dydt_final = dynamics(tf, y(end,:)', tf, alpha, g, D, N, m); + + pT = [1 0 0 0]; + [~, p] = ode45(@(t, p) adjoint(t, p, y, g, t_span, alpha, dDdv, m), flip(t_span), pT); + + v = y(:,3); + DFdu = -dDda(v,alpha')./m; + p3 = p((N:-1:1),3); + gradJ = [-dydt_final(1); -DFdu.*p3]; + +end + +function [ceq] = groundConstraint(u, g, t0, v0, x1_0, x2_0, gamma0, D, dDdv, dDda, N, m) + y0 = [x1_0 x2_0 v0 gamma0]'; + tf = u(1); + t_span = linspace(t0, tf, N); + alpha = u(2:N+1); + [~, y] = ode45(@(t, y) dynamics(t, y, tf, alpha, g, D, N, m), t_span, y0); + + ceq = y(end,2); + dydt_final = dynamics(t_span(end), y(end,:)', tf, alpha, g, D, N, m); + + pT = [0 -1 0 0]; + [~, p] = ode45(@(t, p) adjoint(t, p, y, g, t_span, alpha, dDdv, m), flip(t_span), pT); + + v = y(:,3); + DFdu = -dDda(v,alpha'); + + Dceq = [dydt_final(2); -DFdu.*p(:,3)]; +end + +function dpdt = adjoint(t, p, y, g, t_span, alpha, dDdv, m) + v = interp1(t_span, y(:,3), t); + gamma = interp1(t_span, y(:,4), t); + alpha = interp1(t_span, alpha, t); + + J = [0 0 0 0; + 0 0 0 0; + cos(gamma) sin(gamma) -dDdv(v,alpha)./m (g/v^2)*cos(gamma); + -v*sin(gamma) v*cos(gamma) -g*cos(gamma) (g/v)*sin(gamma)]; + dpdt = -J*p; +end + +function dydt = dynamics(t, y, tf, alpha, g, D, N, m) + v = y(3); + gamma = y(4); + + t_span = linspace(0, tf, N); + alpha = interp1(t_span, alpha, t); + + dydt = [ + v*cos(gamma); + v*sin(gamma); + -g*sin(gamma)-D(v,alpha)/m; + -(g/v)*cos(gamma) + ]; +end diff --git a/OptimalControl/old/drag/nodrag_data.mat b/OptimalControl/old/drag/nodrag_data.mat new file mode 100644 index 0000000000..0bb8bd9ca7 Binary files /dev/null and b/OptimalControl/old/drag/nodrag_data.mat differ diff --git a/OptimalControl/old/nodrag/main_constraint_nodrag.m b/OptimalControl/old/nodrag/main_constraint_nodrag.m new file mode 100644 index 0000000000..f134905faf --- /dev/null +++ b/OptimalControl/old/nodrag/main_constraint_nodrag.m @@ -0,0 +1,155 @@ +function main_constraint_nodrag + persistent u_history J_history; + clc; close all + + g = 9.81; + lb = [0 0.01]; + ub = [10 pi/2]; + + x1_0 = 0; x2_0 = 0; v0 = 15; + gamma0 = deg2rad(40); + t0 = 0; tf = 2; + u0 = [tf; gamma0]; + + cost = @(u) f_cost(u, g, t0, v0, x1_0, x2_0); + constraint = @(u) groundConstraint(u, g, t0, v0, x1_0, x2_0); + + checkGradients(cost, u0,'Tolerance',1e-3) + checkGradients(constraint,u0,'Tolerance',1e-3) + + options = optimoptions("fmincon", ... + "OutputFcn", @store_fmincon, ... + "Algorithm", "sqp", ... + "DerivativeCheck", "on"); + + [u_opt, ~] = fmincon(cost, u0, [], [], [], [], lb, ub, @(u) nonlcon(u,constraint), options); + + y0 = [x1_0 x2_0 v0 u_opt(2)]; + t_span = linspace(t0, u_opt(1), 100); + [~, y] = ode45(@(t, y) dynamics(y, g), t_span, y0); + + disp(["Maximum distance [m] = ", num2str(y(end, 1))]) + disp(["Initial angle [°]: ", num2str(rad2deg(u_opt(2)))]) + disp(["Final Time [s]: ", num2str(u_opt(1))]) + + %%% Results plot %%% + + set(groot, 'defaultTextInterpreter', 'latex'); + set(groot, 'defaultAxesTickLabelInterpreter', 'latex'); + set(groot, 'defaultLegendInterpreter', 'latex'); + + % Trajectory + + figure; + plot(y(:,1), y(:,2), 'b-', 'LineWidth', 2); + hold on + plot(y(1,1), y(1,2), 'go', 'MarkerSize', 7, 'MarkerFaceColor', 'g'); % initial + plot(y(end,1), y(end,2), 'ro', 'MarkerSize', 7, 'MarkerFaceColor', 'r'); % final + xlabel("Horizontal distance [m]"); + ylabel("Vertical distance [m]"); + ylim([0 6]) + grid on; + legend({'Trajectory','Start','Final'}, 'Location', 'Best'); + + % Control over iterations + + figure; + subplot(2,1,1) + plot(u_history(:,1), 'b-o', 'LineWidth', 2, 'MarkerSize', 6); + xlabel('Iteration'); ylabel('Final time [s]'); grid on; + title('Convergence of final time') + + subplot(2,1,2) + plot(rad2deg(u_history(:,2)), 'r-o', 'LineWidth', 2, 'MarkerSize', 6); + xlabel('Iteration'); ylabel('Initial angle [$^circ$]'); grid on; + title('Convergence of initial angle') + + % Cost function + + figure; + plot(J_history, 'k-o', 'LineWidth', 2, 'MarkerSize', 6); + xlabel('Iteration'); ylabel('Cost function J'); grid on; + title('Evolution of cost function') + + vars = whos; + nodrag_results = struct(); + for i = 1:length(vars) + nodrag_results.(vars(i).name) = eval(vars(i).name); + end + save('nodrag_data.mat', 'nodrag_results'); + + function stop = store_fmincon(u, optimValues, state) + if optimValues.iteration == 0 + u_history = []; + J_history = []; + end + if strcmp(state, 'iter') + u_history = [u_history; u(:)']; + J_history = [J_history; optimValues.fval]; + assignin('base', 'u_iterations', u_history); + assignin('base', 'J_iterations', J_history); + end + stop = false; + end +end + +function [c, ceq, Dc, Dceq] = nonlcon(u,constraint) + c = []; + Dc = []; + [ceq, Dceq] = constraint(u); +end + +function [J, gradJ] = f_cost(u, g, t0, v0, x1_0, x2_0) + y0 = [x1_0 x2_0 v0 u(2)]; + tf = u(1); + t_span = linspace(t0, tf, 1000); + [~, y] = ode45(@(t, y) dynamics(y, g), t_span, y0); + + J = -y(end,1); + dydt_final = dynamics(y(end,:)', g); + + pT = [1 0 0 0]; + [~, p] = ode45(@(t, p) adjoint(t, p, y, g, t_span), flip(t_span), pT); + q = p(end,:); + + gradJ = [-dydt_final(1); -q(4)]; +end + +function [ceq, Dceq] = groundConstraint(u, g, t0, v0, x1_0, x2_0) + y0 = [x1_0 x2_0 v0 u(2)]'; + tf = u(1); + t_span = linspace(t0, tf, 1000); + [~, y] = ode45(@(t, y) dynamics(y, g), t_span, y0); + + ceq = y(end,2); + dydt_final = dynamics(y(end,:)', g); + + pT = [0 -1 0 0]; + [~, p] = ode45(@(t, p) adjoint(t, p, y, g, t_span), flip(t_span), pT); + q = p(end,:); + + Dceq = [dydt_final(2); -q(4)]; +end + +function dpdt = adjoint(t, p, y, g, t_span) + ind = round((t - t_span(1)) / (t_span(2) - t_span(1))) + 1; + v = y(ind,3); + gamma = y(ind,4); + + J = [0 0 0 0; + 0 0 0 0; + cos(gamma) sin(gamma) 0 (g/v^2)*cos(gamma); + -v*sin(gamma) v*cos(gamma) -g*cos(gamma) (g/v)*sin(gamma)]; + dpdt = -J*p; +end + +function dydt = dynamics(y, g) + v = y(3); + gamma = y(4); + dydt = [ + v*cos(gamma); + v*sin(gamma); + -g*sin(gamma); + -(g/v)*cos(gamma) + ]; +end diff --git a/OptimalControl/old/nodrag/nodrag_data.mat b/OptimalControl/old/nodrag/nodrag_data.mat new file mode 100644 index 0000000000..007d5e8b97 Binary files /dev/null and b/OptimalControl/old/nodrag/nodrag_data.mat differ diff --git a/nodrag_data.mat b/nodrag_data.mat new file mode 100644 index 0000000000..e41e4d3f51 Binary files /dev/null and b/nodrag_data.mat differ