From 9c4ea37aff1388ef7a83f35e17cac138111663cd Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Fri, 25 Aug 2023 16:55:21 +0200 Subject: [PATCH] Move all discretization options from model to problem_options (#50) * improve pendulum example * move all discretization options from model to problem_options * remove some prints * fix cart_pole_example * add check on T, initialize empty * remove some redundant checks * prioritize T_sim over N_sim --- .../main_simple_car_tutorial.m | 2 +- .../main_tutorial_nosnoc_paper.m | 2 +- .../cart_pole_smoothing.m | 94 ++++---- .../cart_pole_with_friction.m | 3 +- .../get_cart_pole_with_friction_model.m | 49 +++-- .../plot_cart_pole_trajectory.m | 7 +- .../simulate_cart_pole.m | 63 ++++++ .../bouncing_ball_1d_sim.m | 4 +- .../bouncing_ball_2d_sim.m | 4 +- .../bouncing_ball_3d_sim.m | 4 +- examples/cls_minimal_examples/falling_rod.m | 6 +- .../two_simple_contacts.m | 4 +- .../cls_tutorial_ocp/disc_around_obstacle.m | 6 +- examples/cls_tutorial_ocp/three_cart_ocp_2D.m | 8 +- .../three_cart_ocp_2D_stable.m | 8 +- examples/cls_tutorial_ocp/two_cart_ocp_2D.m | 8 +- .../main_switching_cases.m | 6 +- .../simplest_example.m | 4 +- .../disc_manipulation/disc_around_obstacle.m | 6 +- .../disc_manipulation/disc_manipulation.m | 6 +- .../disc_manipulation/disc_switch_position.m | 6 +- .../main_friction_blocks.m | 4 +- examples/gene_regulation/main_irma.m | 4 +- examples/gene_regulation/main_two_gene.m | 4 +- examples/hopper_robot_ocp/hopper_long_ocp.m | 6 +- examples/hopper_robot_ocp/hopper_ocp.m | 6 +- examples/ivp_problem/illustrate_a_solution.m | 4 +- examples/ivp_problem/run_all_experiments.m | 2 +- examples/monoped_ocp/monoped_model.m | 8 +- examples/monoped_ocp/monoped_ocp.m | 10 +- examples/monoped_ocp/monoped_stewart_model.m | 4 +- examples/monoped_ocp/robot_ocp_for_plot.m | 8 +- examples/monoped_ocp/solve_robot_ocp.m | 2 +- .../ocp_voice_coil_motor_with_friction.m | 4 +- ...ce_coil_motor_with_friction_time_optimal.m | 4 +- .../benchmark/integrator_order_experiment.m | 4 +- .../oscilator_simulation_example.m | 6 +- .../periodic_slip_stick_codim1.m | 4 +- .../periodic_slip_stick_codim2.m | 4 +- .../relay_feedback_system.m | 4 +- examples/robot_ocp/robot_ocp.m | 2 +- examples/robot_ocp/robot_ocp_plain.m | 2 +- examples/simplest_example.m | 2 +- .../sliding_mode_ocp/main_sliding_mode_ocp.m | 2 +- .../plots/sliding_mode_ocp_experiment.m | 6 +- .../sliding_mode_ocp_experiment.m | 6 +- .../sliding_mode_ocp/sliding_mode_ocp_plot.m | 8 +- .../sliding_mode_plot_for_paper.m | 4 +- examples/tank_cascade/main_tank_cascade.m | 2 +- examples/three_cart_problem/three_cart_ocp.m | 8 +- examples/three_cart_problem/three_cart_ocp2.m | 8 +- examples/three_cart_problem/three_cart_sim.m | 6 +- .../elastic_ball_in_box_model.m | 2 +- .../main_temp_control_simulation.m | 4 +- .../main_car_hysteresis_ocp.m | 2 +- .../Painleve_paradox.m | 6 +- .../elastic_2d_ball.m | 4 +- .../falling_rod_sim.m | 6 +- .../inelastic_2d_2contacts_sim.m | 4 +- .../inelastic_2d_ocp.m | 2 +- .../inelastic_2d_sim.m | 4 +- .../inelastic_2d_sim_external_force.m | 4 +- .../inelastic_3d_sim.m | 4 +- .../time_freezing_pile_of_balls.m | 4 +- .../main_throwing_ball.m | 2 +- .../run_car_turbo_benchmark.m | 2 +- .../simple_car_example.m | 4 +- .../solve_car_turbo_with_nosnoc.m | 2 +- .../two_balls_spring_benchmark.m | 4 +- .../two_balls_cls/two_balls_spring_example.m | 4 +- src/ControlStage.m | 14 +- src/FiniteElement.m | 7 +- src/NosnocIntegrator.m | 12 +- src/NosnocMPCC.m | 12 +- src/NosnocModel.m | 207 ++++++++---------- src/NosnocProblemOptions.m | 58 +++-- src/NosnocSolver.m | 4 +- src/extract_results_from_solver.m | 4 +- test/TestIntegrator.m | 2 +- test/test_fesd_and_time_options.m | 4 +- test/test_integrator.m | 6 +- test/test_simple_car_model.m | 2 +- 82 files changed, 459 insertions(+), 385 deletions(-) create mode 100644 examples/cart_pole_with_friction/simulate_cart_pole.m diff --git a/examples/a_simple_tutorial/main_simple_car_tutorial.m b/examples/a_simple_tutorial/main_simple_car_tutorial.m index 211bfae6..7855a224 100644 --- a/examples/a_simple_tutorial/main_simple_car_tutorial.m +++ b/examples/a_simple_tutorial/main_simple_car_tutorial.m @@ -17,7 +17,7 @@ model = NosnocModel(); problem_options.N_stages = 10; % number of control intervals problem_options.N_finite_elements = 3; % number of finite element on every control interval (optionally a vector might be passed) -model.T = 1; % Time horizon +problem_options.T = 1; % Time horizon % Symbolic variables and bounds q = SX.sym('q'); v = SX.sym('v'); model.x = [q;v]; % add all important data to the struct model, diff --git a/examples/a_simple_tutorial/main_tutorial_nosnoc_paper.m b/examples/a_simple_tutorial/main_tutorial_nosnoc_paper.m index bdbca45c..014e5c2e 100644 --- a/examples/a_simple_tutorial/main_tutorial_nosnoc_paper.m +++ b/examples/a_simple_tutorial/main_tutorial_nosnoc_paper.m @@ -12,7 +12,7 @@ settings.N_stages = 10; settings.N_finite_elements = 3; -model.T = 1; +problem_options.T = 1; q = SX.sym('q'); v = SX.sym('v'); model.x = [q;v]; model.x0 = [0;0]; model.lbx = [-inf;-25]; model.ubx = [inf;25]; diff --git a/examples/cart_pole_with_friction/cart_pole_smoothing.m b/examples/cart_pole_with_friction/cart_pole_smoothing.m index db62382a..a72f7545 100644 --- a/examples/cart_pole_with_friction/cart_pole_smoothing.m +++ b/examples/cart_pole_with_friction/cart_pole_smoothing.m @@ -36,11 +36,12 @@ %% n_s = 1; N = 30; % number of control intervals +T = 5; % time horizon N_FE = 2; % number of integration steps = Finite elements, without switch detections x_ref = [0; 180/180*pi; 0; 0]; % target position %% NLP formulation & solver creation -nlp = setup_collocation_nlp(model, n_s, N, N_FE); +nlp = setup_collocation_nlp(model, T, N, n_s, N_FE); casadi_nlp = struct('f', nlp.f, 'x', nlp.w, 'p', nlp.p, 'g', nlp.g); solver = nlpsol('solver', 'ipopt', casadi_nlp); @@ -64,12 +65,11 @@ results = struct(); results.x = [q1_opt; q2_opt; v1_opt; v2_opt]; -results.t_grid = linspace(0, model.T, N+1); -results.t_grid_u = linspace(0, model.T, N+1); +results.t_grid = linspace(0, T, N+1); +results.t_grid_u = linspace(0, T, N+1); results.u = u_opt; -model.h_k = model.T / N; -plot_cart_pole_trajectory(results, model, x_ref); +plot_cart_pole_trajectory(results, T/N, x_ref); %% figure; @@ -109,45 +109,9 @@ ylabel('objective', 'Interpreter', 'latex') end -function [nlp, idx_x_shooting_nodes] = setup_collocation_nlp(model, n_s, N, N_FE) +function nlp = setup_collocation_nlp(model, T, N, n_s, N_FE) import casadi.* - %% collocation using casadi - % Get collocation points - tau_root = [0 collocation_points(n_s, 'radau')]; - - % Coefficients of the collocation equation - C = zeros(n_s+1,n_s+1); - - % Coefficients of the continuity equation - D = zeros(n_s+1, 1); - - % Coefficients of the quadrature function - B = zeros(n_s+1, 1); - - % Construct polynomial basis - for j=1:n_s+1 - % Construct Lagrange polynomials to get the polynomial basis at the collocation point - coeff = 1; - for r=1:n_s+1 - if r ~= j - coeff = conv(coeff, [1, -tau_root(r)]); - coeff = coeff / (tau_root(j)-tau_root(r)); - end - end - % Evaluate the polynomial at the final time to get the coefficients of the continuity equation - D(j) = polyval(coeff, 1.0); - - % Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation - pder = polyder(coeff); - for r=1:n_s+1 - C(j,r) = polyval(pder, tau_root(r)); - end - - % Evaluate the integral of the polynomial to get the coefficients of the quadrature function - pint = polyint(coeff); - B(j) = polyval(pint, 1.0); - end - + [B, C, D] = generate_collocation_coefficients(n_s); %% Extract from model L = model.f_q; @@ -159,7 +123,7 @@ f_terminal = Function('f', {model.x,}, {model.f_q_T}); % Control discretization - h = model.T/(N*N_FE); + h = T/(N*N_FE); x0 = model.x0; %% Casadi NLP formulation @@ -234,7 +198,6 @@ lbg = [lbg; zeros(n_x,1)]; ubg = [ubg; zeros(n_x,1)]; end - n_w = length(vertcat(w{:})); end objective = objective + f_terminal(Xk); @@ -250,3 +213,44 @@ nlp.lbg = lbg; nlp.ubg = ubg; end + + +function [B, C, D] = generate_collocation_coefficients(n_s) + import casadi.* + %% collocation using casadi + % Get collocation points + tau_root = [0 collocation_points(n_s, 'radau')]; + + % Coefficients of the collocation equation + C = zeros(n_s+1,n_s+1); + + % Coefficients of the continuity equation + D = zeros(n_s+1, 1); + + % Coefficients of the quadrature function + B = zeros(n_s+1, 1); + + % Construct polynomial basis + for j=1:n_s+1 + % Construct Lagrange polynomials to get the polynomial basis at the collocation point + coeff = 1; + for r=1:n_s+1 + if r ~= j + coeff = conv(coeff, [1, -tau_root(r)]); + coeff = coeff / (tau_root(j)-tau_root(r)); + end + end + % Evaluate the polynomial at the final time to get the coefficients of the continuity equation + D(j) = polyval(coeff, 1.0); + + % Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation + pder = polyder(coeff); + for r=1:n_s+1 + C(j,r) = polyval(pder, tau_root(r)); + end + + % Evaluate the integral of the polynomial to get the coefficients of the quadrature function + pint = polyint(coeff); + B(j) = polyval(pint, 1.0); + end +end diff --git a/examples/cart_pole_with_friction/cart_pole_with_friction.m b/examples/cart_pole_with_friction/cart_pole_with_friction.m index 7e1cda51..2556b07a 100644 --- a/examples/cart_pole_with_friction/cart_pole_with_friction.m +++ b/examples/cart_pole_with_friction/cart_pole_with_friction.m @@ -34,6 +34,7 @@ % Discretization options problem_options = NosnocProblemOptions(); +problem_options.T = 5; % Time horizon problem_options.irk_scheme = IRKSchemes.RADAU_IIA; problem_options.n_s = 3; problem_options.dcs_mode = 'Stewart'; @@ -62,4 +63,4 @@ disp(['final difference to desired angle: ', num2str(distance_to_target(2), '%.3e'), ' rad']) % visualtize -plot_cart_pole_trajectory(results, model, x_ref) +plot_cart_pole_trajectory(results, problem_options.h_k(1), x_ref) diff --git a/examples/cart_pole_with_friction/get_cart_pole_with_friction_model.m b/examples/cart_pole_with_friction/get_cart_pole_with_friction_model.m index 1bac25a7..44b5b320 100644 --- a/examples/cart_pole_with_friction/get_cart_pole_with_friction_model.m +++ b/examples/cart_pole_with_friction/get_cart_pole_with_friction_model.m @@ -32,7 +32,6 @@ else model = struct(); end - model.T = 4; % Time horizon % fixed values m1 = 1; % cart @@ -41,38 +40,45 @@ link_length = 1; % symbolics - q = SX.sym('q', 2); - v = SX.sym('v', 2); - x = [q;v]; - u = SX.sym('u', 1); % control + px = SX.sym('px'); + theta = SX.sym('theta'); + q = vertcat(px, theta); + + v = SX.sym('v'); + theta_dot = SX.sym('theta_dot'); + q_dot = vertcat(v, theta_dot); + + x = vertcat(q, q_dot); + + u = SX.sym('u'); % control % Inertia matrix - M = [m1 + m2, m2*link_length*cos(q(2));... - m2 *link_length*cos(q(2)), m2*link_length^2]; + M = [m1 + m2, m2*link_length*cos(theta);... + m2 *link_length*cos(theta), m2*link_length^2]; % Coriolis force - C = [0, -m2 * link_length*v(2)*sin(q(2));... + C = [0, -m2 * link_length*theta_dot*sin(theta);... 0, 0]; % all forces = Gravity + Control + Coriolis (+Friction) - f_all = [0; -m2*g*link_length*sin(x(2))] + [u; 0] - C*v; + f_all = [0; -m2*g*link_length*sin(theta)] + [u; 0] - C*q_dot; if nosnoc % switching function c: cart velocity - model.c = v(1); + model.c = v; % Sign matrix % f_1 for c>0, f_2 for c<0 model.S = [1; -1]; % Dynamics for v > 0 - f_1 = [v;... - inv(M)*(f_all-[F_friction;0])]; + f_1 = [q_dot;... + inv(M)*(f_all-[F_friction; 0])]; % Dynamics for v<0 - f_2 = [v;... - inv(M)*(f_all+[F_friction;0])]; + f_2 = [q_dot;... + inv(M)*(f_all+[F_friction; 0])]; model.F = [f_1, f_2]; else sigma = SX.sym('sigma'); model.p = sigma; - model.f_expl_ode = [v; inv(M) * (f_all - [F_friction*tanh(v(1)/ sigma); 0])]; + model.f_expl_ode = [q_dot; inv(M) * (f_all - [F_friction*tanh(v/ sigma); 0])]; end % specify initial and desired state @@ -80,16 +86,17 @@ x_ref = [0; 180/180*pi; 0; 0]; % end upwards % bounds - ubx = [5; 240/180*pi; 20; 20]; - lbx = [-0.0; -240/180*pi; -20; -20]; + ubx = [5; inf; inf; inf]; + lbx = [-5; -inf; -inf; -inf]; u_max = 30; - u_ref = 0; % cost - Q = diag([1; 100; 1; 1]); - Q_terminal = diag([100; 100; 10; 10]); + Q = diag([10; 100; 1; 1]); R = 1; - model.f_q = (x-x_ref)'*Q*(x-x_ref)+ (u-u_ref)'*R*(u-u_ref); + model.f_q = (x-x_ref)'*Q*(x-x_ref)+ u'*R*u; + + % terminal cost could be added + Q_terminal = diag([500; 100; 10; 10]); model.f_q_T = (x-x_ref)'*Q_terminal*(x-x_ref); model.lbx = lbx; diff --git a/examples/cart_pole_with_friction/plot_cart_pole_trajectory.m b/examples/cart_pole_with_friction/plot_cart_pole_trajectory.m index 6a574f87..5137b2ef 100644 --- a/examples/cart_pole_with_friction/plot_cart_pole_trajectory.m +++ b/examples/cart_pole_with_friction/plot_cart_pole_trajectory.m @@ -25,7 +25,7 @@ % This file is part of NOSNOC. -function plot_cart_pole_trajecetory(results, model, x_ref) +function plot_cart_pole_trajectory(results, time_step, x_ref) link_length = 1; % extract results @@ -81,9 +81,9 @@ function plot_cart_pole_trajecetory(results, model, x_ref) im = frame2im(frame); [imind,cm] = rgb2ind(im,256); if ii == 1 - imwrite(imind,cm,filename,'gif', 'Loopcount', inf,'DelayTime', model.h_k(1)); + imwrite(imind,cm,filename,'gif', 'Loopcount', inf,'DelayTime', time_step); else - imwrite(imind,cm,filename,'gif', 'WriteMode', 'append','DelayTime', model.h_k(1)); + imwrite(imind,cm,filename,'gif', 'WriteMode', 'append','DelayTime', time_step); end if ii 0 - obj.augmented_objective = obj.augmented_objective + (model.T/problem_options.N_stages)*model.f_lsq_u_fun(obj.Uk,model.u_ref_val(:,obj.ctrl_idx), p_stage); - obj.objective = obj.objective + (model.T/problem_options.N_stages)*model.f_lsq_u_fun(obj.Uk,model.u_ref_val(:,obj.ctrl_idx), p_stage); + obj.augmented_objective = obj.augmented_objective + (problem_options.T/problem_options.N_stages)*model.f_lsq_u_fun(obj.Uk,model.u_ref_val(:,obj.ctrl_idx), p_stage); + obj.objective = obj.objective + (problem_options.T/problem_options.N_stages)*model.f_lsq_u_fun(obj.Uk,model.u_ref_val(:,obj.ctrl_idx), p_stage); end % TODO: combine this into a function if problem_options.use_fesd && problem_options.equidistant_control_grid if ~problem_options.time_optimal_problem - obj.addConstraint(sum(vertcat(obj.stage.h)) - model.h, 'type', 'stage'); + obj.addConstraint(sum(vertcat(obj.stage.h)) - problem_options.h, 'type', 'stage'); elseif ~problem_options.time_freezing if problem_options.use_speed_of_time_variables - obj.addConstraint(sum(vertcat(obj.stage.h)) - model.h, 'type', 'stage') + obj.addConstraint(sum(vertcat(obj.stage.h)) - problem_options.h, 'type', 'stage') obj.addConstraint(sum(s_sot*vertcat(obj.stage.h)) - T_final/problem_options.N_stages, 'type', 'stage'); else obj.addConstraint(sum(vertcat(obj.stage.h)) - T_final/problem_options.N_stages, 'type', 'stage'); @@ -177,7 +177,7 @@ if problem_options.time_optimal_problem obj.addConstraint(fe.x{end}(end) - ctrl_idx*(T_final/problem_options.N_stages) + model.x0(end), 'type', 'stage'); else - obj.addConstraint(fe.x{end}(end) - ctrl_idx*model.h + model.x0(end), 'type', 'stage'); + obj.addConstraint(fe.x{end}(end) - ctrl_idx*problem_options.h + model.x0(end), 'type', 'stage'); end end end diff --git a/src/FiniteElement.m b/src/FiniteElement.m index 2feb6502..04fb5625 100644 --- a/src/FiniteElement.m +++ b/src/FiniteElement.m @@ -90,7 +90,6 @@ cross_comp_pairs all_comp_pairs n_comp_components - ctrl_idx fe_idx @@ -256,7 +255,7 @@ if problem_options.use_fesd h = define_casadi_symbolic(problem_options.casadi_symbolic_mode, ['h_' num2str(ctrl_idx-1) '_' num2str(fe_idx-1)]); - h_ctrl_stage = model.T/problem_options.N_stages; + h_ctrl_stage = problem_options.T/problem_options.N_stages; h0 = h_ctrl_stage / problem_options.N_finite_elements(ctrl_idx); ubh = (1 + problem_options.gamma_h) * h0; lbh = (1 - problem_options.gamma_h) * h0; @@ -1013,7 +1012,7 @@ elseif obj.problem_options.time_optimal_problem && ~obj.problem_options.use_speed_of_time_variables h = obj.T_final/(obj.problem_options.N_stages*obj.problem_options.N_finite_elements(obj.ctrl_idx)); else - h = obj.model.T/(obj.problem_options.N_stages*obj.problem_options.N_finite_elements(obj.ctrl_idx)); + h = obj.problem_options.T/(obj.problem_options.N_stages*obj.problem_options.N_finite_elements(obj.ctrl_idx)); end end @@ -1414,7 +1413,7 @@ function stepEquilibration(obj, rho_h_p) % only heuristic mean is done for first finite element if problem_options.step_equilibration == StepEquilibrationMode.heuristic_mean - h_fe = model.T / (sum(problem_options.N_finite_elements)); % TODO this may be a bad idea if using different N_fe. may want to issue warning in that case + h_fe = problem_options.T / (sum(problem_options.N_finite_elements)); % TODO this may be a bad idea if using different N_fe. may want to issue warning in that case obj.augmented_objective = obj.augmented_objective + rho_h_p * (obj.h - h_fe).^2; return; elseif obj.fe_idx <= 1 diff --git a/src/NosnocIntegrator.m b/src/NosnocIntegrator.m index dfffb402..0170a379 100644 --- a/src/NosnocIntegrator.m +++ b/src/NosnocIntegrator.m @@ -114,7 +114,7 @@ %% Main simulation loop - for ii = 1:model.N_sim + for ii = 1:problem_options.N_sim %% set problem parameters, controls solver.set("x0", x0); @@ -132,7 +132,7 @@ solver.set('x', {x0}) solver.set('x_left_bp', {x0}) if exist('initial_guess', 'var') - t_guess = t_current + cumsum([0; model.h_k * ones(problem_options.N_finite_elements, 1)]); + t_guess = t_current + cumsum([0; problem_options.h_k * ones(problem_options.N_finite_elements, 1)]); x_guess = interp1(initial_guess.t_grid, initial_guess.x_traj, t_guess,'makima'); lambda_normal_guess = interp1(initial_guess.t_grid, initial_guess.lambda_normal_traj, t_guess(2:end-1), 'makima'); % @@ -188,7 +188,7 @@ end elseif solver_options.print_level >=2 fprintf('Integration step %d / %d (%2.3f s / %2.3f s) converged in %2.3f s. \n',... - ii, model.N_sim, t_current, model.T_sim, solver_stats.cpu_time_total); + ii, problem_options.N_sim, t_current, problem_options.T_sim, solver_stats.cpu_time_total); end %% gather results @@ -253,10 +253,10 @@ %% update inital value x0 = res.x(:,end); - t_current = t_current + model.T; + t_current = t_current + problem_options.T; % update clock state if problem_options.impose_terminal_phyisical_time - solver.nlp.p0(end) = solver.nlp.p0(end)+model.T; + solver.nlp.p0(end) = solver.nlp.p0(end)+problem_options.T; end end @@ -271,7 +271,7 @@ end fprintf('---------------- Stats summary ----------------------------\n'); fprintf('N_sim\t step-size\t\tN_stg\tN_FE\t CPU Time (s)\t Max. CPU (s)/iter\tMin. CPU (s)/iter\tMax. comp.\tMin. comp.\n'); - fprintf('%d\t\t\t%2.3f\t\t%d\t\t%d\t\t%2.3f\t\t\t\t%2.3f\t\t\t%2.3f\t\t\t\t%2.2e\t%2.2e\n', model.N_sim, model.h_sim, problem_options.N_stages, problem_options.N_finite_elements(1), total_time, max(time_per_iter), min(time_per_iter), max(complementarity_stats), min(complementarity_stats)); + fprintf('%d\t\t\t%2.3f\t\t%d\t\t%d\t\t%2.3f\t\t\t\t%2.3f\t\t\t%2.3f\t\t\t\t%2.2e\t%2.2e\n', problem_options.N_sim, problem_options.h_sim, problem_options.N_stages, problem_options.N_finite_elements(1), total_time, max(time_per_iter), min(time_per_iter), max(complementarity_stats), min(complementarity_stats)); fprintf('-----------------------------------------------------------------\n\n'); %% Output diff --git a/src/NosnocMPCC.m b/src/NosnocMPCC.m index 75750c74..f4e718b6 100644 --- a/src/NosnocMPCC.m +++ b/src/NosnocMPCC.m @@ -154,7 +154,7 @@ ind_x_all end - methods + methods function obj = NosnocMPCC(problem_options, model) import casadi.* tic; @@ -269,7 +269,7 @@ % the final time in time optimal control problems T_final = define_casadi_symbolic(problem_options.casadi_symbolic_mode, 'T_final', 1); obj.T_final = T_final; - T_final_guess = model.T; + T_final_guess = problem_options.T; end % Add global vars @@ -303,7 +303,7 @@ end if problem_options.equidistant_control_grid && ~problem_options.stagewise_clock_constraint if ~problem_options.time_optimal_problem - obj.addConstraint(last_fe.x{end}(end)-model.T, 'type', 'g_mpcc'); + obj.addConstraint(last_fe.x{end}(end)-problem_options.T, 'type', 'g_mpcc'); end end else @@ -344,7 +344,7 @@ end end if ~problem_options.time_optimal_problem - obj.addConstraint(sum_h_all-model.T, 0, 0, 'type', 'g_mpcc'); + obj.addConstraint(sum_h_all-problem_options.T, 0, 0, 'type', 'g_mpcc'); else if ~problem_options.use_speed_of_time_variables obj.addConstraint(sum_h_all-T_final, 0, 0, 'type', 'g_mpcc'); @@ -362,7 +362,7 @@ end end % T_num = T_phy = T_final \neq T. - obj.addConstraint(sum_h_all-model.T, 0, 0, 'type', 'g_mpcc'); + obj.addConstraint(sum_h_all-problem_options.T, 0, 0, 'type', 'g_mpcc'); obj.addConstraint(integral_clock_state-T_final, 0, 0, 'type', 'g_mpcc'); end end @@ -500,7 +500,7 @@ obj.objective_fun = Function('objective_fun', {obj.w, obj.p}, {obj.objective}); obj.g_fun = Function('g_fun', {obj.w, obj.p}, {obj.g}); - obj.p0 = [problem_options.rho_sot; problem_options.rho_h; problem_options.rho_terminal; model.T]; + obj.p0 = [problem_options.rho_sot; problem_options.rho_h; problem_options.rho_terminal; problem_options.T]; if dims.n_p_global > 0 obj.p0 = [obj.p0; model.p_global_val]; diff --git a/src/NosnocModel.m b/src/NosnocModel.m index 37a21dba..94fd20e1 100644 --- a/src/NosnocModel.m +++ b/src/NosnocModel.m @@ -44,13 +44,10 @@ S % Sign matrix g_Stewart % Stewart indicator functions g_ind - + f_q % Stage cost f_q_T % Terminal cost - h % Step size - h_k % Finite element step size - % least squares lsq_x x_ref @@ -77,14 +74,6 @@ g_terminal_lb g_terminal_ub - % Terminal time - T = 1.0 - - % Sim - T_sim - N_sim - h_sim - %----- DCS/time_freezing mode user input ----- f_c % Gap functions mu_f % Friction coef @@ -216,14 +205,14 @@ obj.dims = NosnocDimensions(); end - function generate_equations(obj, settings) + function generate_equations(obj, problem_options) if obj.equations_exist return end import casadi.* dims = obj.dims; - dcs_mode = settings.dcs_mode; - + dcs_mode = problem_options.dcs_mode; + %% Model functions of the DCS mode % if f_x doesnt exist we generate it from F % if it does we are in expert mode. TODO name. @@ -238,9 +227,9 @@ function generate_equations(obj, settings) case 'Step' obj.f_x = obj.f_x + obj.F{ii}*obj.theta_step_sys{ii}; case 'CLS' - if ~settings.lift_velocity_state + if ~problem_options.lift_velocity_state if obj.friction_exists - switch settings.friction_model + switch problem_options.friction_model case 'Conic' F_v = inv(obj.M)*(obj.f_v+obj.J_normal*obj.lambda_normal+obj.J_tangent*obj.lambda_tangent); case 'Polyhedral' @@ -253,7 +242,7 @@ function generate_equations(obj, settings) else obj.f_x = [v;obj.z_v]; if obj.friction_exists - switch settings.friction_model + switch problem_options.friction_model case 'Conic' g_lift_v = obj.M*obj.z_v -(obj.f_v +obj.J_normal*obj.lambda_normal + obj.J_tangent*obj.lambda_tangent); case 'Polyhedral' @@ -304,8 +293,8 @@ function generate_equations(obj, settings) lambda00_expr = [lambda00_expr; -min(obj.c{ii}, 0); max(obj.c{ii},0)]; case 'CLS' % dumy variables for impact quations: - v_post_impact = define_casadi_symbolic(settings.casadi_symbolic_mode,'v_post_impact',dims.n_q); - v_pre_impact = define_casadi_symbolic(settings.casadi_symbolic_mode,'v_pre_impact',dims.n_q); + v_post_impact = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'v_post_impact',dims.n_q); + v_pre_impact = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'v_pre_impact',dims.n_q); g_alg_cls = [g_alg_cls; obj.y_gap - obj.f_c]; g_impulse = [g_impulse; obj.M*(v_post_impact-v_pre_impact)-obj.J_normal*obj.Lambda_normal]; % TODO: can this be relaxed? velocity junction g_impulse = [g_impulse; obj.Y_gap-obj.f_c]; @@ -315,7 +304,7 @@ function generate_equations(obj, settings) g_impulse = [g_impulse; obj.L_vn(ii) - obj.J_normal(:,ii)'*(v_post_impact+obj.e(ii)*v_pre_impact)]; end if obj.friction_exists - switch settings.friction_model + switch problem_options.friction_model case 'Conic' g_impulse(1:dims.n_q) = obj.M*(v_post_impact-v_pre_impact)-obj.J_normal*obj.Lambda_normal-obj.J_tangent*obj.Lambda_tangent; % algebraic and friction equations @@ -327,7 +316,7 @@ function generate_equations(obj, settings) -obj.J_tangent(:,ind_temp)'*v_post_impact - 2*obj.Gamma(ii)*obj.Lambda_tangent(ind_temp);... obj.Beta - ((obj.mu_f(ii)*obj.Lambda_normal(ii))^2- norm(obj.Lambda_tangent(ind_temp))^2)]; - if ~isequal(settings.conic_model_switch_handling,'Plain') + if ~isequal(problem_options.conic_model_switch_handling,'Plain') % equality constraints for pos and neg parts of the tangetial velocity g_alg_cls = [g_alg_cls;obj.J_tangent(:,ind_temp)'*obj.v-(obj.p_vt(ind_temp)-obj.n_vt(ind_temp))]; g_impulse = [g_impulse;obj.J_tangent(:,ind_temp)'*v_post_impact - (obj.P_vt(ind_temp)-obj.N_vt(ind_temp))]; @@ -351,7 +340,7 @@ function generate_equations(obj, settings) obj.f_c_fun = Function('f_c_fun', {obj.x}, {obj.f_c}); obj.J_normal_fun = Function('J_normal_fun', {obj.x}, {obj.J_normal}); if obj.friction_exists - if isequal(settings.friction_model,'Conic') + if isequal(problem_options.friction_model,'Conic') obj.J_tangent_fun = Function('J_tangent_fun', {obj.x}, {obj.J_tangent}); else obj.D_tangent_fun = Function('D_tangent_fun', {obj.x}, {obj.D_tangent}); @@ -429,13 +418,13 @@ function generate_equations(obj, settings) obj.equations_exist = 1; end - function generate_variables(obj,settings) + function generate_variables(obj,problem_options) if obj.vars_exist return end import casadi.* - casadi_symbolic_mode = settings.casadi_symbolic_mode; - dcs_mode = settings.dcs_mode; + casadi_symbolic_mode = problem_options.casadi_symbolic_mode; + dcs_mode = problem_options.dcs_mode; dims = obj.dims; g_lift_theta_step = []; @@ -502,10 +491,10 @@ function generate_variables(obj,settings) theta_temp = []; ii_str = num2str(ii); S_temp = obj.S{ii}; - if settings.pss_lift_step_functions + if problem_options.pss_lift_step_functions % TODO implement automatic lifting else - if ~settings.time_freezing_inelastic + if ~problem_options.time_freezing_inelastic for j = 1:size(S_temp,1) alpha_ij = 1; for k = 1:size(S_temp,2) @@ -523,7 +512,7 @@ function generate_variables(obj,settings) end %% time-freezing inelastic impacts (exploit structure with taiolored formulae) - if settings.time_freezing_inelastic + if problem_options.time_freezing_inelastic % theta_step are the lifting variables that enter the ODE r.h.s. if any(obj.mu_f > 0) obj.friction_exists = 1; @@ -531,7 +520,7 @@ function generate_variables(obj,settings) obj.friction_exists = 0; end - if ~settings.nonsmooth_switching_fun + if ~problem_options.nonsmooth_switching_fun alpha_q = obj.alpha(1:dims.n_contacts); alpha_v_normal = obj.alpha(dims.n_contacts+1:2*dims.n_contacts); if obj.friction_exists @@ -558,9 +547,9 @@ function generate_variables(obj,settings) beta_prod_expr = []; beta_prod_expr_guess = []; % extra expresion to make depend only on alpha (the one above depens on both and alpha and beta) - needed for eval. of inital guess - if settings.pss_lift_step_functions + if problem_options.pss_lift_step_functions % lift bilinear terms in product terms for free flight ode % (alpha_q*alpha_v) - if ~settings.nonsmooth_switching_fun + if ~problem_options.nonsmooth_switching_fun beta_bilinear_ode = define_casadi_symbolic(casadi_symbolic_mode,'beta_bilinear_ode',dims.n_contacts); beta_bilinear_ode_expr = eval([casadi_symbolic_mode '.zeros(' num2str(dims.n_contacts) ',1);']); if obj.friction_exists @@ -581,9 +570,9 @@ function generate_variables(obj,settings) % expresions for theta's and lifting %% Filippov multipliers alpha_ode = 1; % initalized product for free flight multiplier - if ~settings.pss_lift_step_functions + if ~problem_options.pss_lift_step_functions for ii = 1:dims.n_contacts - if settings.nonsmooth_switching_fun + if problem_options.nonsmooth_switching_fun alpha_ode = alpha_ode*alpha_qv(ii); if obj.friction_exists theta_step_expr(ii+1) = (1-alpha_qv(ii))*(alpha_v_tangent(ii)); @@ -604,7 +593,7 @@ function generate_variables(obj,settings) theta_step_expr(1) = alpha_ode; else % lift and have bilinear terms - if settings.nonsmooth_switching_fun + if problem_options.nonsmooth_switching_fun if dims.n_contacts <= 2 for ii = 1:dims.n_contacts alpha_ode = alpha_ode*alpha_qv(ii); @@ -689,7 +678,7 @@ function generate_variables(obj,settings) obj.lambda_tangent = define_casadi_symbolic(casadi_symbolic_mode,'lambda_tangent',dims.n_tangents); % Impulse varaibles obj.Lambda_tangent = define_casadi_symbolic(casadi_symbolic_mode,'Lambda_tangent',dims.n_tangents); - if isequal(settings.friction_model,'Polyhedral') + if isequal(problem_options.friction_model,'Polyhedral') obj.gamma_d = define_casadi_symbolic(casadi_symbolic_mode,'gamma_d',dims.n_contacts); obj.beta_d = define_casadi_symbolic(casadi_symbolic_mode,'beta_d',dims.n_contacts); % lift friction cone bound obj.delta_d = define_casadi_symbolic(casadi_symbolic_mode,'delta_d',dims.n_tangents); % lift lagrangian @@ -698,13 +687,13 @@ function generate_variables(obj,settings) obj.Beta_d = define_casadi_symbolic(casadi_symbolic_mode,'Beta_d',dims.n_contacts); % lift friction cone bound obj.Delta_d = define_casadi_symbolic(casadi_symbolic_mode,'Delta_d',dims.n_tangents); % lift lagrangian end - if isequal(settings.friction_model,'Conic') + if isequal(problem_options.friction_model,'Conic') obj.gamma = define_casadi_symbolic(casadi_symbolic_mode,'gamma',dims.n_contacts); obj.beta = define_casadi_symbolic(casadi_symbolic_mode,'beta',dims.n_contacts); % Impulse variables; obj.Gamma = define_casadi_symbolic(casadi_symbolic_mode,'Gamma',dims.n_contacts); obj.Beta = define_casadi_symbolic(casadi_symbolic_mode,'Beta',dims.n_contacts); - switch settings.conic_model_switch_handling + switch problem_options.conic_model_switch_handling case 'Plain' % no extra constraints case 'Abs' @@ -749,18 +738,18 @@ function generate_variables(obj,settings) % Impulse obj.z_impulse = [obj.z_impulse;obj.Lambda_tangent]; % only for dcs_mode = cls, note that they are evaluated only at left boundary point of at FE % friction aux multipliers - if isequal(settings.friction_model,'Polyhedral') + if isequal(problem_options.friction_model,'Polyhedral') % polyhedral friction model algebaric variables obj.z_all = [obj.z_all;obj.gamma_d;obj.beta_d;obj.delta_d]; % Polyhedral friction - collect impulse variables obj.z_impulse = [obj.z_impulse;obj.Gamma_d;obj.Beta_d;obj.Delta_d]; end - if isequal(settings.friction_model,'Conic') + if isequal(problem_options.friction_model,'Conic') % conic friction model algebaric variables obj.z_all = [obj.z_all;obj.gamma;obj.beta]; % Conic impulse obj.z_impulse = [obj.z_impulse;obj.Gamma;obj.Beta]; - switch settings.conic_model_switch_handling + switch problem_options.conic_model_switch_handling case 'Plain' % no extra constraints case 'Abs' @@ -774,7 +763,7 @@ function generate_variables(obj,settings) end end end - if settings.lift_velocity_state + if problem_options.lift_velocity_state obj.z_v = define_casadi_symbolic(casadi_symbolic_mode,['z_v'],dims.n_q); obj.z_all = [obj.z_all;obj.z_v]; end @@ -784,26 +773,14 @@ function generate_variables(obj,settings) obj.vars_exist = 1; end - - function verify_and_backfill(obj, settings) + + function verify_and_backfill(obj, problem_options) import casadi.* - if settings.time_freezing - obj.time_freezing(settings); + if problem_options.time_freezing + obj.time_freezing(problem_options); end dims = obj.dims; - if ~isempty(obj.N_sim) && ~isempty(obj.T_sim) - obj.T = obj.T_sim/obj.N_sim; - obj.h_sim = obj.T_sim/(obj.N_sim*settings.N_stages*settings.N_finite_elements); - if settings.print_level >= 2 && exist("h_sim") - fprintf('Info: N_sim is given, so the h_sim provided by the user is overwritten.\n') - end - elseif ~isempty(obj.N_sim) || ~isempty(obj.T_sim) - error('Provide both N_sim and T_sim for the integration.') - end - obj.h = obj.T/settings.N_stages; - obj.h_k = obj.h./settings.N_finite_elements; - if size(obj.x, 1) ~= 0 dims.n_x = length(obj.x); % check lbx @@ -825,7 +802,7 @@ function verify_and_backfill(obj, settings) else error('nosnoc: Please provide the state vector x, a CasADi symbolic variable.'); end - settings.casadi_symbolic_mode = ['casadi.' obj.x(1).type_name()]; + problem_options.casadi_symbolic_mode = ['casadi.' obj.x(1).type_name()]; %% Check is u provided if size(obj.u, 1) ~= 0 @@ -855,10 +832,10 @@ function verify_and_backfill(obj, settings) obj.u0 = 0*ones(dims.n_u,1); end else - obj.u = define_casadi_symbolic(settings.casadi_symbolic_mode,'',0); + obj.u = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'',0); obj.u0 = []; dims.n_u = 0; - if settings.print_level >=1 + if problem_options.print_level >=1 fprintf('nosnoc: No control vector u is provided. \n') end obj.lbu = []; @@ -896,7 +873,7 @@ function verify_and_backfill(obj, settings) obj.z0 = []; obj.lbz = []; obj.ubz = []; - obj.z = define_casadi_symbolic(settings.casadi_symbolic_mode,'',0); + obj.z = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'',0); end %% Global vars (i.e., variables that do not change with time) if size(obj.v_global, 1) ~= 0 @@ -926,7 +903,7 @@ function verify_and_backfill(obj, settings) end else n_v_global = 0; - obj.v_global = define_casadi_symbolic(settings.casadi_symbolic_mode, '', 0); + obj.v_global = define_casadi_symbolic(problem_options.casadi_symbolic_mode, '', 0); obj.v0_global = []; obj.lbv_global = []; obj.ubv_global = []; @@ -944,48 +921,42 @@ function verify_and_backfill(obj, settings) end else dims.n_p_global = 0; - obj.p_global = define_casadi_symbolic(settings.casadi_symbolic_mode,'',0); + obj.p_global = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'',0); obj.p_global_val = []; end if size(obj.p_time_var, 1) ~= 0 dims.n_p_time_var = size(obj.p_time_var, 1); if size(obj.p_time_var_val, 1) ~= 0 - if size(obj.p_time_var_val) ~= [dims.n_p_time_var, settings.N_stages] + if size(obj.p_time_var_val) ~= [dims.n_p_time_var, problem_options.N_stages] error('nosnoc: User provided p_global_val has the wrong size.') end else - obj.p_time_var_val = zeros(dims.n_p_time_var, settings.N_stages); + obj.p_time_var_val = zeros(dims.n_p_time_var, problem_options.N_stages); end obj.p_time_var_stages = []; - for ii=1:settings.N_stages - var_full = define_casadi_symbolic(settings.casadi_symbolic_mode, ['p_time_var_' num2str(ii)], dims.n_p_time_var); + for ii=1:problem_options.N_stages + var_full = define_casadi_symbolic(problem_options.casadi_symbolic_mode, ['p_time_var_' num2str(ii)], dims.n_p_time_var); obj.p_time_var_stages = horzcat(obj.p_time_var_stages, var_full); end else dims.n_p_time_var = 0; - obj.p_time_var = define_casadi_symbolic(settings.casadi_symbolic_mode,'',0); - obj.p_time_var_stages = define_casadi_symbolic(settings.casadi_symbolic_mode,'', [0, settings.N_stages]); - obj.p_time_var_val = double.empty(0,settings.N_stages); + obj.p_time_var = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'',0); + obj.p_time_var_stages = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'', [0, problem_options.N_stages]); + obj.p_time_var_val = double.empty(0,problem_options.N_stages); end obj.p = vertcat(obj.p_global,obj.p_time_var); %% Stage and terminal costs check if ~size(obj.f_q, 1) ~= 0 - if settings.print_level >=1 - fprintf('nosnoc: No stage cost is provided. \n') - end obj.f_q = 0; end if size(obj.f_q_T, 1) ~= 0 terminal_cost = 1; else - if settings.print_level >=1 - fprintf('nosnoc: No terminal cost is provided. \n') - end obj.f_q_T = 0; end %% Least squares objective terms with variables references @@ -1002,23 +973,23 @@ function verify_and_backfill(obj, settings) n_x_ref_rows = size(obj.lsq_x{2},1); n_x_ref_cols = size(obj.lsq_x{2},2); - if n_x_ref_cols == settings.N_stages + if n_x_ref_cols == problem_options.N_stages fprintf('nosnoc: the provided reference for the differential states is time variable. \n'); elseif n_x_ref_cols == 1 % replaciate fprintf('nosnoc: the provided reference for the differential states is constant over time. \n'); - obj.lsq_x{2} = repmat(obj.lsq_x{2},1,settings.N_stages); + obj.lsq_x{2} = repmat(obj.lsq_x{2},1,problem_options.N_stages); else - fprintf('nosnoc: The reference in lsq_x has to have a length of %d (if constant) or %d if time vriables. \n',1,settings.N_stages) + fprintf('nosnoc: The reference in lsq_x has to have a length of %d (if constant) or %d if time vriables. \n',1,problem_options.N_stages) error('nosnoc: Please provide x_ref in lsq_x{1} with an appropriate size.') end obj.x_ref_val = obj.lsq_x{2}; - obj.x_ref = define_casadi_symbolic(settings.casadi_symbolic_mode,'x_ref',n_x_ref_rows); + obj.x_ref = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'x_ref',n_x_ref_rows); obj.f_lsq_x = (obj.lsq_x{1}-obj.x_ref)'*obj.lsq_x{3}*(obj.lsq_x{1}-obj.x_ref); else - obj.x_ref = define_casadi_symbolic(settings.casadi_symbolic_mode,'x_ref',1); + obj.x_ref = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'x_ref',1); obj.f_lsq_x = 0; - obj.x_ref_val = zeros(1,settings.N_stages); + obj.x_ref_val = zeros(1,problem_options.N_stages); end % least square terms for control inputs @@ -1034,23 +1005,23 @@ function verify_and_backfill(obj, settings) end n_u_ref_rows = size(obj.lsq_u{2},1); n_u_ref_cols = size(obj.lsq_u{2},2); - if n_u_ref_cols == settings.N_stages + if n_u_ref_cols == problem_options.N_stages fprintf('nosnoc: the provided reference for the control inputs is time variable. \n'); elseif n_u_ref_cols == 1 % replaciate fprintf('nosnoc: the provided reference for the control inputs is constant over time. \n'); - obj.lsq_u{2} = repmat(obj.lsq_u{2},1,settings.N_stages); + obj.lsq_u{2} = repmat(obj.lsq_u{2},1,problem_options.N_stages); else - fprintf('nosnoc: The reference in lsq_u has to have a length of %d (if constant) or %d if time vriables. \n',1,settings.N_stages) + fprintf('nosnoc: The reference in lsq_u has to have a length of %d (if constant) or %d if time vriables. \n',1,problem_options.N_stages) error('nosnoc: Please provide u_ref in lsq_u{2} with an appropriate size.') end obj.u_ref_val = obj.lsq_u{2}; - obj.u_ref = define_casadi_symbolic(settings.casadi_symbolic_mode,'u_ref',n_u_ref_rows); + obj.u_ref = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'u_ref',n_u_ref_rows); obj.f_lsq_u = (obj.lsq_u{1}-obj.u_ref)'*obj.lsq_u{3}*(obj.lsq_u{1}-obj.u_ref); else - obj.u_ref = define_casadi_symbolic(settings.casadi_symbolic_mode,'u_ref',1); + obj.u_ref = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'u_ref',1); obj.f_lsq_u = 0; - obj.u_ref_val = zeros(1,settings.N_stages); + obj.u_ref_val = zeros(1,problem_options.N_stages); end @@ -1076,10 +1047,10 @@ function verify_and_backfill(obj, settings) error('nosnoc: Please provide a reference vector in lsq_T{2} with an appropriate size.') end obj.x_ref_end_val = obj.lsq_T{2}; - obj.x_ref_end = define_casadi_symbolic(settings.casadi_symbolic_mode,'x_ref_end',n_x_T_rows); + obj.x_ref_end = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'x_ref_end',n_x_T_rows); obj.f_lsq_T = (obj.lsq_T{1}-obj.x_ref_end)'*obj.lsq_T{3}*(obj.lsq_T{1}-obj.x_ref_end); else - obj.x_ref_end = define_casadi_symbolic(settings.casadi_symbolic_mode,'x_ref_end',1); + obj.x_ref_end = define_casadi_symbolic(problem_options.casadi_symbolic_mode,'x_ref_end',1); obj.f_lsq_T = 0; obj.x_ref_end_val = 0; end @@ -1144,7 +1115,7 @@ function verify_and_backfill(obj, settings) c_all = []; obj.friction_exists = 0; - if isequal(settings.dcs_mode,'CLS') + if isequal(problem_options.dcs_mode,'CLS') % TODO: there is some repetition to the time_freezing check, this should be unified!!!! % Check existence of relevant functions dims.n_sys = 1; % always one subystem in CLS (only loops over n_contacts later) @@ -1239,7 +1210,7 @@ function verify_and_backfill(obj, settings) % Tangent Contact Jacobian if obj.friction_exists - if isequal(settings.friction_model,'Conic') + if isequal(problem_options.friction_model,'Conic') if size(obj.J_tangent, 1) ~= 0 if size(obj.J_tangent,1)~=dims.n_q error('nosnoc: J_tangent has the wrong size.') @@ -1249,7 +1220,7 @@ function verify_and_backfill(obj, settings) end end - if isequal(settings.friction_model,'Polyhedral') + if isequal(problem_options.friction_model,'Polyhedral') if isempty(obj.D_tangent) error('nosnoc: please provide the polyhedral tangent Jacobian in model.D_tangent, e.g., using the conic tangent Jacobian model.J_tangent: D_tangent = [J_tangent(q_0),-J_tangent(q_0)].') end @@ -1258,9 +1229,9 @@ function verify_and_backfill(obj, settings) % Dimension of tangents dims.n_t = 0; if obj.friction_exists - if isequal(settings.friction_model,'Polyhedral') + if isequal(problem_options.friction_model,'Polyhedral') dims.n_t = size(obj.D_tangent,2)/dims.n_contacts; % number of tanget multipliers for a single contactl - elseif isequal(settings.friction_model,'Conic') + elseif isequal(problem_options.friction_model,'Conic') dims.n_t = size(obj.J_tangent,2)/dims.n_contacts; % number of tanget multipliers for a single contactl end dims.n_tangents = dims.n_t*dims.n_contacts; % number tangent forces for all multpliers @@ -1269,7 +1240,7 @@ function verify_and_backfill(obj, settings) end end - if isequal(settings.dcs_mode,'Step') || isequal(settings.dcs_mode,'Stewart') + if isequal(problem_options.dcs_mode,'Step') || isequal(problem_options.dcs_mode,'Stewart') if isempty(obj.F) % Don't need F if ~obj.general_inclusion @@ -1293,7 +1264,7 @@ function verify_and_backfill(obj, settings) if ~obj.general_inclusion % if the matrix S is not provided, maybe the g_ind are available % directly? - if isequal(settings.dcs_mode,'Stewart') + if isequal(problem_options.dcs_mode,'Stewart') if ~isempty(obj.g_ind) if ~iscell(obj.g_ind) obj.g_ind = {obj.g_ind}; @@ -1302,15 +1273,15 @@ function verify_and_backfill(obj, settings) for ii = 1:dims.n_sys % discriminant functions obj.g_Stewart{ii} = obj.g_ind{ii}; - obj.c{ii} = zeros(1,settings.casadi_symbolic_mode); - c_all = [c_all; zeros(1,settings.casadi_symbolic_mode)]; + obj.c{ii} = zeros(1,problem_options.casadi_symbolic_mode); + c_all = [c_all; zeros(1,problem_options.casadi_symbolic_mode)]; end else error(['nosnoc: Neither the sign matrix S nor the indicator functions g_ind for regions are provided. ' ... 'Either provide the matrix S and the expression for c, or the expression for g_ind.']); end else - error(['nosnoc: The user uses settings.dcs_mode = ''Step'', but the sign matrix S is not provided. Please provide the matrix S and the expressions for c(x) (definfing the region boundaries).']); + error(['nosnoc: The user uses problem_options.dcs_mode = ''Step'', but the sign matrix S is not provided. Please provide the matrix S and the expressions for c(x) (definfing the region boundaries).']); end else if isempty(obj.c) @@ -1351,13 +1322,13 @@ function verify_and_backfill(obj, settings) end % check are the matrices dense - if isequal(settings.dcs_mode,'Stewart') + if isequal(problem_options.dcs_mode,'Stewart') for ii = 1:dims.n_sys if any(sum(abs(obj.S{ii}),2)=1 - fprintf('nosnoc: settings.pss_lift_step_functions set to 0, as are step fucntion selections are already entering the ODE linearly.\n') + if problem_options.print_level >=1 + fprintf('nosnoc: problem_options.pss_lift_step_functions set to 0, as are step fucntion selections are already entering the ODE linearly.\n') end end @@ -1403,10 +1374,10 @@ function verify_and_backfill(obj, settings) end % populate dims - obj.dims.n_s = settings.n_s; + obj.dims.n_s = problem_options.n_s; end - function time_freezing(obj,settings) + function time_freezing(obj,problem_options) import casadi.* dims = obj.dims; if ~isempty(obj.F) @@ -1546,7 +1517,7 @@ function time_freezing(obj,settings) % qudrature state dims.n_quad = 0; - if settings.time_freezing_quadrature_state + if problem_options.time_freezing_quadrature_state % define quadrature state L = define_casadi_symbolic(casadi_symbolic_mode,'L',1); if ~isempty(obj.lbx) @@ -1606,11 +1577,11 @@ function time_freezing(obj,settings) end %% Time-freezing reformulation if obj.e == 0 - % Basic settings - settings.time_freezing_inelastic = 1; % flag tha inealstic time-freezing is using (for hand crafted lifting) - settings.dcs_mode = 'Step'; % time freezing inelastic works better step (very inefficient with stewart) + % Basic problem_options + problem_options.time_freezing_inelastic = 1; % flag tha inealstic time-freezing is using (for hand crafted lifting) + problem_options.dcs_mode = 'Step'; % time freezing inelastic works better step (very inefficient with stewart) %% switching function - if settings.nonsmooth_switching_fun + if problem_options.nonsmooth_switching_fun obj.c = [max_smooth_fun(obj.f_c,v_normal,0);v_tangent]; else if dims.n_dim_contact == 2 @@ -1637,8 +1608,8 @@ function time_freezing(obj,settings) f_aux_pos = []; % matrix wit all aux tan dyn f_aux_neg = []; % time freezing dynamics - if settings.stabilizing_q_dynamics - f_q_dynamics = -settings.kappa_stabilizing_q_dynamics*obj.J_normal*diag(obj.f_c); + if problem_options.stabilizing_q_dynamics + f_q_dynamics = -problem_options.kappa_stabilizing_q_dynamics*obj.J_normal*diag(obj.f_c); else f_q_dynamics = zeros(dims.n_q,dims.n_contacts); end @@ -1680,7 +1651,7 @@ function time_freezing(obj,settings) dcs_mode = 'Step'; if isempty(obj.k_aux) obj.k_aux = 10; - if settings.print_level > 1 + if problem_options.print_level > 1 fprintf('nosnoc: Setting default value for k_aux = 10.\n') end end diff --git a/src/NosnocProblemOptions.m b/src/NosnocProblemOptions.m index efda6d43..612f80de 100644 --- a/src/NosnocProblemOptions.m +++ b/src/NosnocProblemOptions.m @@ -25,12 +25,20 @@ % This file is part of NOSNOC. classdef NosnocProblemOptions < handle -% TODO clean up much of the work here. properties % General use_fesd(1,1) logical = 1 casadi_symbolic_mode {mustBeMember(casadi_symbolic_mode,{'casadi.SX', 'casadi.MX'})} = 'casadi.SX' + % In case of simulation problem + T_sim + N_sim + h_sim + + h % Step size + h_k % Finite element step size + T % Terminal time % TODO: why is there also T_val, do we need both? + % descritization N_stages(1,1) {mustBeInteger, mustBePositive} = 1; N_finite_elements {mustBeInteger, mustBePositive} = 2; @@ -75,9 +83,9 @@ time_optimal_problem(1,1) = 0 % Step equilibration - rho_h(1,1) double {mustBeReal, mustBePositive} = 1 + rho_h(1,1) double {mustBePositive} = 1 step_equilibration(1,1) StepEquilibrationMode = StepEquilibrationMode.heuristic_mean % heuristic_mean, l2_relaxed, l2_relaxed_scaled, direct, direct_homotopy, direct_homotopy_lift - step_equilibration_sigma(1,1) double {mustBeReal, mustBePositive} = 0.1 % slope at zero in rescaling the indicator function, nu_ki_rescaled = tanh(nu_ki/step_equilibration_sigma) + step_equilibration_sigma(1,1) double {mustBePositive} = 0.1 % slope at zero in rescaling the indicator function, nu_ki_rescaled = tanh(nu_ki/step_equilibration_sigma) % Multiple shooting type discretization equidistant_control_grid(1,1) logical = 1 @@ -90,13 +98,13 @@ local_speed_of_time_variable(1,1) logical = 0 stagewise_clock_constraint(1,1) logical = 1 impose_terminal_phyisical_time(1,1) logical = 1 - s_sot0(1,1) double {mustBeReal, mustBePositive} = 1 - s_sot_max(1,1) double {mustBeReal, mustBePositive} = 25 - s_sot_min(1,1) double {mustBeReal, mustBePositive} = 1 - S_sot_nominal(1,1) double {mustBeReal, mustBePositive} = 1 + s_sot0(1,1) double {mustBePositive} = 1 + s_sot_max(1,1) double {mustBePositive} = 25 + s_sot_min(1,1) double {mustBePositive} = 1 + S_sot_nominal(1,1) double {mustBePositive} = 1 rho_sot(1,1) double {mustBeReal, mustBeNonnegative} = 0 - T_final_max(1,1) double {mustBeReal, mustBePositive} = 1e2 + T_final_max(1,1) double {mustBePositive} = 1e2 T_final_min(1,1) double {mustBeReal, mustBeNonnegative} = 0 time_freezing_reduced_model(1,1) logical = 0 % analytic reduction of lifter formulation, less algebraic variables (experimental) time_freezing_hysteresis(1,1) logical = 0 % do not do automatic time freezing generation for hysteresis, it is not supported yet. @@ -109,7 +117,7 @@ nonsmooth_switching_fun(1,1) logical = 0 % experimental: use c = max(c1,c2) insetad of c = [c1c2] % expert mode: stabilize auxiliary dynamics in \nabla f_c(q) direction stabilizing_q_dynamics(1,1) logical = 0 - kappa_stabilizing_q_dynamics(1,1) double {mustBeReal, mustBePositive} = 1e-5 + kappa_stabilizing_q_dynamics(1,1) double {mustBePositive} = 1e-5 % Verbose print_level = 3 @@ -124,14 +132,14 @@ % Relaxation of terminal constraint relax_terminal_constraint(1,1) {mustBeInteger, mustBeInRange(relax_terminal_constraint, 0, 3)} = 0 % 0 - hard constraint, 1 - ell_1 , 2 - ell_2 , 3 - ell_inf TODO enum relax_terminal_constraint_from_above(1,1) logical = 0 - rho_terminal(1,1) double {mustBeReal, mustBePositive} = 1e2 + rho_terminal(1,1) double {mustBePositive} = 1e2 relax_terminal_constraint_homotopy(1,1) logical = 0 % terminal penalty is governed by homotopy parameter % Experimental: no_initial_impacts(1,1) logical = 0 % All MPCC parameters - T_val(1,1) double {mustBeReal, mustBePositive} = 1 + T_val(1,1) double {mustBePositive} = 1 p_val % Butcher Tableu @@ -142,22 +150,44 @@ D_irk double c_irk double - right_boundary_point_explicit(1,1) logical % TODO this shoud live in model probably + right_boundary_point_explicit(1,1) logical end properties(Dependent) - time_rescaling + time_rescaling end methods function obj = NosnocProblemOptions() - obj.p_val = [obj.rho_sot,obj.rho_h,obj.rho_terminal,obj.T_val]; + obj.p_val = [obj.rho_sot, obj.rho_h, obj.rho_terminal, obj.T_val]; end function [] = preprocess(obj) import casadi.* + % time grid + % TODO: merge T_sim and T? + if ~isempty(obj.N_sim) && ~isempty(obj.T_sim) + obj.T = obj.T_sim/obj.N_sim; + obj.h_sim = obj.T_sim/(obj.N_sim*obj.N_stages*obj.N_finite_elements); + if obj.print_level >= 2 && exist("h_sim") + fprintf('Info: N_sim is given, so the h_sim provided by the user is overwritten.\n') + end + elseif ~isempty(obj.N_sim) || ~isempty(obj.T_sim) + error('Provide both N_sim and T_sim for the integration.') + end + + if numel(obj.T) ~= 1 && ~obj.time_optimal_problem + error('terminal numerical time T must be provided if time_optimal_problem is False.'); + elseif numel(obj.T) == 0 && ~obj.time_optimal_problem + obj.T = 1; + elseif numel(obj.T) ~= 1 + error('terminal time T must be a positive scalar.'); + end + obj.h = obj.T/obj.N_stages; + obj.h_k = obj.h./obj.N_finite_elements; + % check irk scheme compatibility if ismember(obj.irk_scheme, IRKSchemes.differential_only) if obj.print_level >=1 diff --git a/src/NosnocSolver.m b/src/NosnocSolver.m index fd7c78c4..8c1ffed8 100644 --- a/src/NosnocSolver.m +++ b/src/NosnocSolver.m @@ -621,7 +621,7 @@ function print_solver_stats(obj, results, stats) if mpcc.problem_options.time_optimal_problem T_opt = w_mpcc(mpcc.ind_t_final); else - T_opt = mpcc.model.T; + T_opt = mpcc.problem_options.T; end results.T = T_opt; @@ -633,7 +633,7 @@ function print_solver_stats(obj, results, stats) % T = T_opt; % end for ii = 1:obj.mpcc.problem_options.N_stages - h_opt = [h_opt,obj.mpcc.model.T/(obj.mpcc.problem_options.N_stages*obj.mpcc.problem_options.N_finite_elements(ii))*ones(1, obj.mpcc.problem_options.N_finite_elements(ii))]; + h_opt = [h_opt,obj.mpcc.problem_options.T/(obj.mpcc.problem_options.N_stages*obj.mpcc.problem_options.N_finite_elements(ii))*ones(1, obj.mpcc.problem_options.N_finite_elements(ii))]; end end results.h = h_opt; diff --git a/src/extract_results_from_solver.m b/src/extract_results_from_solver.m index e0bc99e8..bb79809f 100644 --- a/src/extract_results_from_solver.m +++ b/src/extract_results_from_solver.m @@ -23,7 +23,7 @@ if settings.time_optimal_problem T_opt = w_opt(problem.ind_t_final); else - T_opt = problem.model.T; + T_opt = problem.problem_options.T; end results.T = T_opt; @@ -35,7 +35,7 @@ % T = T_opt; % end for ii = 1:settings.N_stages - h_opt = [h_opt,model.T/(settings.N_stages*settings.N_finite_elements(ii))*ones(1, settings.N_finite_elements(ii))]; + h_opt = [h_opt,problem_options.T/(settings.N_stages*settings.N_finite_elements(ii))*ones(1, settings.N_finite_elements(ii))]; end end results.h = h_opt; diff --git a/test/TestIntegrator.m b/test/TestIntegrator.m index 1f7e5c78..295c7fc8 100644 --- a/test/TestIntegrator.m +++ b/test/TestIntegrator.m @@ -7,7 +7,7 @@ irk_scheme = {IRKSchemes.RADAU_IIA,IRKSchemes.GAUSS_LEGENDRE}; dcs_mode = {'Step','Stewart'}; end - + methods (Test) end methods (Test, ParameterCombination = 'exhaustive') diff --git a/test/test_fesd_and_time_options.m b/test/test_fesd_and_time_options.m index 7722aa27..1fcd43ee 100644 --- a/test/test_fesd_and_time_options.m +++ b/test/test_fesd_and_time_options.m @@ -47,9 +47,9 @@ % Add terminal constraint model.g_terminal = [q-200;v-0]; if time_optimal_problem - model.T = 1; + problem_options.T = 1; else - model.T = 25; + problem_options.T = 25; model.f_q = u^2; end % Solve OCP diff --git a/test/test_integrator.m b/test/test_integrator.m index 520deaff..ddb2b023 100644 --- a/test/test_integrator.m +++ b/test/test_integrator.m @@ -29,8 +29,8 @@ model = NosnocModel(); problem_options.N_finite_elements = N_finite_elements; -model.T_sim = T_sim; -model.N_sim = N_sim; +problem_options.T_sim = T_sim; +problem_options.N_sim = N_sim; omega = -2*pi; A1 = [1 omega;... -omega 1]; @@ -56,5 +56,5 @@ % numerical error x_fesd = results.x(:,end); error_x = norm(x_fesd-x_star,"inf"); -fprintf(['Numerical error with h = %2.3f and ' char(problem_options.irk_scheme) ' with n_s = %d stages is: %5.2e: \n'],model.h_sim,problem_options.n_s,error_x); +fprintf(['Numerical error with h = %2.3f and ' char(problem_options.irk_scheme) ' with n_s = %d stages is: %5.2e: \n'],problem_options.h_sim,problem_options.n_s,error_x); end diff --git a/test/test_simple_car_model.m b/test/test_simple_car_model.m index cdd6fd47..0355b549 100644 --- a/test/test_simple_car_model.m +++ b/test/test_simple_car_model.m @@ -19,7 +19,7 @@ model = NosnocModel(); problem_options.N_stages = 10; % number of control intervals problem_options.N_finite_elements = 3; % number of finite element on every control interval (optionally a vector might be passed) -model.T = 1; % Time horizon +problem_options.T = 1; % Time horizon % Symbolic variables and bounds q = SX.sym('q'); v = SX.sym('v'); model.x = [q;v]; % add all important data to the struct model,