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

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,

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];

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

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)

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,55 +40,63 @@
     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
     x0 = [1; 0/180*pi; 0; 0]; % start downwards
     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;

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<length(q1_opt)
@@ -116,5 +116,4 @@ function plot_cart_pole_trajecetory(results, model, x_ref)
     ylabel('$u(t)$','Interpreter','latex')
     xlabel('$t$','Interpreter','latex')
     grid on
-    xlim([0 model.T])
 end

examples/cart_pole_with_friction/simulate_cart_pole.m

@@ -0,0 +1,63 @@
+% BSD 2-Clause License
+
+% Copyright (c) 2022, Armin Nurkanović, Jonathan Frey, Anton Pozharskiy, Moritz Diehl
+
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+
+% 1. Redistributions of source code must retain the above copyright notice, this
+%    list of conditions and the following disclaimer.
+
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+%    this list of conditions and the following disclaimer in the documentation
+%    and/or other materials provided with the distribution.
+
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+% DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+% FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+% DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+% SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+% CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+% OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+% OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+% This file is part of NOSNOC.
+clear all
+% simple simulation
+F_friction = 2.0;
+model = get_cart_pole_with_friction_model(0, F_friction);
+
+Nsim = 30;
+Tsim = 5;
+h = Tsim/Nsim;
+
+%% create casadi integrator
+import casadi.*
+p_integrator = vertcat(model.u, model.p);
+ode = struct('x', model.x, 'p', p_integrator, 'ode', model.f_expl_ode);
+Phi = integrator('F', 'idas', ode, struct('tf', h));
+
+nx = length(model.x);
+% define parameters
+p_val = ones(length(p_integrator), 1);
+p_traj = repmat(p_val, 1, Nsim);
+p_traj(1, Nsim/2:end) = -2;
+
+% x trajectory
+xcurrent = zeros(nx, 1);
+simX = zeros(nx, Nsim+1);
+simX(:, 1) = xcurrent;
+% simulation loop
+for i = 1:Nsim
+    out = Phi('x0', xcurrent, 'p', p_traj(:, i));
+    xcurrent = full(out.xf);
+    simX(:, i+1) = xcurrent;
+end
+
+% plot
+t_grid = linspace(0, Tsim, Nsim+1);
+results = struct('x', simX, 't_grid', t_grid, 't_grid_u', t_grid, 'u', p_traj(1, :));
+x_ref = zeros(4, 1);
+plot_cart_pole_trajectory(results, h, x_ref);

examples/cls_minimal_examples/bouncing_ball_1d_sim.m

@@ -39,8 +39,8 @@
 T_sim = 3;
 N_sim = 10;
 
-model.T_sim = T_sim;
-model.N_sim = N_sim;
+problem_options.T_sim = T_sim;
+problem_options.N_sim = N_sim;
 problem_options.N_finite_elements = N_FE;
 
 %% Analytic solution

examples/cls_minimal_examples/bouncing_ball_2d_sim.m

@@ -48,9 +48,9 @@
 N_FE = 2;
 T_sim = 1.7;
 N_sim = 10;
-model.T_sim = T_sim;
+problem_options.T_sim = T_sim;
 problem_options.N_finite_elements = N_FE;
-model.N_sim = N_sim;
+problem_options.N_sim = N_sim;
 
 %% Call nosnoc Integrator
 integrator = NosnocIntegrator(model, problem_options, solver_options, [], []);

examples/cls_minimal_examples/bouncing_ball_3d_sim.m

@@ -44,9 +44,9 @@
 N_finite_elements = 10;
 T_sim = 2;
 N_sim = 1;
-model.T_sim = T_sim;
+problem_options.T_sim = T_sim;
 problem_options.N_finite_elements = N_finite_elements;
-model.N_sim = N_sim;
+problem_options.N_sim = N_sim;
 solver_options.use_previous_solution_as_initial_guess = 0;
 %% Call FESD Integrator
 integrator = NosnocIntegrator(model, problem_options, solver_options, [], []);