New design PDS/FOSwP pipeline

+nosnoc/+dcs/Gcs.m

@@ -0,0 +1,64 @@
+classdef Gcs < nosnoc.dcs.Base
+    properties
+        lambda
+
+        f_x
+
+        dims
+
+        nabla_c_fun
+        c_fun
+        g_comp_path_fun
+    end
+
+    methods
+        function obj = Gcs(model)
+            obj.model = model;
+            obj.dims = model.dims;
+        end
+
+        function generate_variables(obj, opts)
+            import casadi.*
+            dims = obj.dims;
+            % dimensions
+            dims.n_lambda = dims.n_c;
+            obj.lambda = define_casadi_symbolic(opts.casadi_symbolic_mode,'lambda',dims.n_lambda);
+
+            obj.dims = dims;
+        end
+
+        function generate_equations(obj, opts)
+            import casadi.*
+            model = obj.model;
+            dims = obj.dims;
+
+            nabla_c = model.c.jacobian(model.x)';
+
+            obj.f_x = model.f_x + model.E*nabla_c*obj.lambda;
+
+            obj.f_x_fun = Function('f_x', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {obj.f_x, model.f_q});
+            obj.f_q_fun = Function('f_q', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {model.f_q});
+            obj.g_z_fun = Function('g_z', {model.x, model.z, model.u, model.v_global, model.p}, {model.g_z});
+            obj.g_alg_fun = Function('g_alg', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {[]});
+            obj.c_fun = Function('c_fun', {model.x, model.z, model.v_global, model.p}, {model.c});
+            obj.nabla_c_fun = Function('c_fun', {model.x, model.z, model.v_global, model.p}, {nabla_c});
+            obj.g_path_fun = Function('g_path', {model.x, model.z, model.u, model.v_global, model.p}, {model.g_path}); % TODO(@anton) do dependence checking for spliting the path constriants
+            obj.g_comp_path_fun  = Function('g_comp_path', {model.x, model.z, model.u, model.v_global, model.p}, {[model.G_path,model.H_path]});
+            obj.g_terminal_fun  = Function('g_terminal', {model.x, model.z, model.v_global, model.p_global}, {model.g_terminal});
+            obj.f_q_T_fun = Function('f_q_T', {model.x, model.z, model.v_global, model.p}, {model.f_q_T});
+            obj.f_lsq_x_fun = Function('f_lsq_x_fun',{model.x,model.x_ref,model.p},{model.f_lsq_x});
+            obj.f_lsq_u_fun = Function('f_lsq_u_fun',{model.u,model.u_ref,model.p},{model.f_lsq_u});
+            obj.f_lsq_T_fun = Function('f_lsq_T_fun',{model.x,model.x_ref_end,model.p_global},{model.f_lsq_T});
+        end
+    end
+
+    methods(Access=protected)
+        function propgrp = getPropertyGroups(obj)
+            propgrp = getPropertyGroups@nosnoc.dcs.Base(obj);
+            group_title = 'Variables';
+            var_list = struct;
+            var_list.lambda;
+            propgrp(2) = matlab.mixin.util.PropertyGroup(var_list, group_title);
+        end
+    end
+end

+nosnoc/+model/Pds.m

@@ -0,0 +1,43 @@
+classdef Pds < nosnoc.model.Base
+    properties
+        f_x
+        c
+        E
+    end
+
+    methods
+        function obj = Pds()
+            obj = obj@nosnoc.model.Base();
+        end
+
+        function verify_and_backfill(obj, opts)
+            arguments
+                obj
+                opts nosnoc.Options
+            end
+            import casadi.*
+            verify_and_backfill@nosnoc.model.Base(obj,opts);
+
+            dims = obj.dims;
+
+            if size(obj.f_x,1) ~= dims.n_x
+                error("nosnoc: f_x has incorrect dimension. It must have the same dimension as x.")
+            end
+
+            dims.n_c = size(obj.c,1);
+
+            if ~isempty(obj.E)
+                if size(obj.E, 1) ~= dims.n_x
+                    error("nosnoc: Projection matrix E must be an n_x by n_x matrix where n_x is the number of functions defining the set C.")
+                end
+                if size(obj.E, 2) ~= dims.n_x
+                    error("nosnoc: Projection matrix E must be an n_x by n_x matrix where n_x is the number of functions defining the set C.")
+                end
+            else
+                obj.E = eye(dims.n_c);
+            end
+
+            obj.dims = dims;
+        end
+    end
+end

+nosnoc/+ocp/Solver.m

@@ -46,7 +46,11 @@
               case "nosnoc.model.Cls"
                 error("not implemented")
               case "nosnoc.model.Pds"
-                error("not implemented")
+                obj.dcs = nosnoc.dcs.Gcs(model);
+                obj.dcs.generate_variables(opts);
+                obj.dcs.generate_equations(opts);
+                obj.discrete_time_problem = nosnoc.discrete_time_problem.Gcs(obj.dcs, opts);
+                obj.discrete_time_problem.populate_problem();
               otherwise
                 error("Unknown model type")
             end

+nosnoc/Integrator.m

@@ -39,7 +39,7 @@
                     obj.dcs.generate_equations(opts);
                     obj.discrete_time_problem = nosnoc.discrete_time_problem.Stewart(obj.dcs, opts);
                     obj.discrete_time_problem.populate_problem();
-                elseif opts.dcs_mode == DcsMode.Heaviside % TODO: RENAME
+                elseif opts.dcs_mode == DcsMode.Heaviside
                     obj.dcs = nosnoc.dcs.Heaviside(model);
                     obj.dcs.generate_variables(opts);
                     obj.dcs.generate_equations(opts);
@@ -48,16 +48,20 @@
                 else
                     error("PSS models can only be reformulated using the Stewart or Heaviside Step reformulations.")
                 end
-              case "nosnoc.model.heaviside"
+              case "nosnoc.model.Heaviside"
                 obj.dcs = nosnoc.dcs.Heaviside(model);
                 obj.dcs.generate_variables(opts);
                 obj.dcs.generate_equations(opts);
                 obj.discrete_time_problem = nosnoc.discrete_time_problem.Heaviside(obj.dcs, opts);
                 obj.discrete_time_problem.populate_problem();
-              case "nosnoc.model.cls"
-                error("not implemented")
-              case "nosnoc.model.pds"
+              case "nosnoc.model.Cls"
                 error("not implemented")
+              case "nosnoc.model.Pds"
+                obj.dcs = nosnoc.dcs.Gcs(model);
+                obj.dcs.generate_variables(opts);
+                obj.dcs.generate_equations(opts);
+                obj.discrete_time_problem = nosnoc.discrete_time_problem.Gcs(obj.dcs, opts);
+                obj.discrete_time_problem.populate_problem();
             end
         end
 

examples/new_design_examples/new_design_car.m

@@ -8,8 +8,8 @@
 solver_options = nosnoc.solver.Options();
 
 %% set some options
-problem_options.rk_scheme = RKSchemes.RADAU_IIA;
-problem_options.rk_representation = RKRepresentation.integral;
+problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC;
+problem_options.rk_representation = RKRepresentation.differential_lift_x;
 problem_options.n_s = 2;
 problem_options.N_stages = 50; % number of control intervals
 problem_options.N_finite_elements = 2; % number of finite element on every control interval (optionally a vector might be passed)

examples/new_design_examples/new_design_moving_set.m

@@ -0,0 +1,46 @@
+
+clear all
+clc
+close all
+import casadi.*
+%% discretization parameters
+N_sim = 100;
+T_sim = 10;
+
+%% NOSNOC settings
+problem_options = nosnoc.Options();
+solver_options = nosnoc.solver.Options();
+problem_options.n_s = 3;
+%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE;
+problem_options.rk_scheme = RKSchemes.RADAU_IIA;
+%problem_options.rk_representation= 'differential_lift_x'; 
+problem_options.rk_representation= 'integral';
+problem_options.N_sim = N_sim;
+problem_options.N_finite_elements = 2;
+problem_options.T_sim = T_sim;
+
+%solver_options.homotopy_steering_strategy = 'ELL_INF';
+solver_options.print_level = 2;
+
+model = nosnoc.model.Pds();
+
+model.x0 = [0;pi-2;0];
+x = SX.sym('x',2);
+t = SX.sym('t');
+model.x = [x;t];
+model.c = [-norm(x - [sin(t);cos(t)])^2+(1-0.5)^2];
+model.f_x = [0; 0; 1];
+model.E = diag([1,1,0]);
+
+integrator = nosnoc.Integrator(model, problem_options, solver_options);
+[t_grid, x_res] = integrator.simulate();
+%
+figure
+plot(x_res(1,:), x_res(2,:))
+grid on
+xlabel('$t$','Interpreter','latex')
+ylabel('$x(t)$','Interpreter','latex')
+grid on
+
+fig = figure('Position', [10 10 1600 800]);
+plot_moving_set([],x_res,[0.5], ["circle"], fig, 'moving_set');

examples/new_design_examples/new_design_msv.m

@@ -0,0 +1,98 @@
+clear all
+close all
+import casadi.*
+import vdx.*
+
+R = 3.5;
+%% Define projected system
+problem_options = nosnoc.Options();
+solver_options = nosnoc.solver.Options();
+%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE;
+%problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC;
+problem_options.rk_scheme = RKSchemes.RADAU_IIA;
+%problem_options.rk_representation= RKRepresentation.differential_lift_x; 
+problem_options.rk_representation = RKRepresentation.integral;
+problem_options.cross_comp_mode = CrossCompMode.STAGE_STAGE;
+problem_options.cross_comp_mode = CrossCompMode.FE_FE;
+problem_options.N_finite_elements = 2;
+problem_options.n_s = 2;
+problem_options.N_stages = 25;
+problem_options.T = 1;
+problem_options.rho_h = 1e-4;
+problem_options.time_optimal_problem = true;
+%solver_options.homotopy_steering_strategy = 'ELL_INF';
+solver_options.complementarity_tol = 1e-10;
+solver_options.print_level = 3;
+
+model = nosnoc.model.Pds();
+x1 = SX.sym('x1', 2);
+x2 = SX.sym('x2', 2);
+x = [x1;x2];
+x_target = [0;0;0;0];
+model.x = [x];
+model.lbx = [-inf;-inf;-inf;-inf];
+model.ubx = [inf;inf;inf;inf];
+x0 =[-25;-25;-15;-15];
+model.x0 = [x0];
+u1 = SX.sym('u1', 2);
+u2 = SX.sym('u2', 2);
+model.u = [u1;u2];
+model.lbu = [-100/sqrt(2);-100/sqrt(2);-60/sqrt(2);-60/sqrt(2)];
+model.ubu = [100/sqrt(2);100/sqrt(2);60/sqrt(2);60/sqrt(2)];
+%model.lbu = [-100/sqrt(2);-100/sqrt(2);0;0];
+%model.ubu = [100/sqrt(2);100/sqrt(2);0;0];
+model.u0 = model.ubu;
+model.c = [norm_2(x2-x1)-2*R];
+model.f_x = [u1;u2];
+
+% costs
+model.f_q = 0;
+model.f_q_T = 0.5*(x-x_target)'*(x-x_target);
+
+% Solve
+ocp_solver = nosnoc.ocp.Solver(model, problem_options, solver_options);
+ocp_solver.solve();
+
+%% plot
+fontsize = 12;
+x_res = ocp_solver.get('x');
+u_res = ocp_solver.get('u');
+u_rep = kron(u_res, ones(1,problem_options.N_finite_elements(1)));
+T_final = ocp_solver.get('T_final');
+lambda_res = ocp_solver.get('lambda');
+h_res = ocp_solver.get('h');
+t_res = [0,cumsum(h_res)];
+c_fun = casadi.Function('nabla_c', {x}, {model.c});
+nabla_c_fun = casadi.Function('nabla_c', {x}, {model.c.jacobian(x)'});
+c_res = full(c_fun(x_res(1:4,:)));
+nabla_c_res = full(nabla_c_fun(x_res(1:4,:)));
+%plot_discs(h_res,x_res,[3.5,3.5], ["circle", "circle"])
+v_res = u_rep + repmat(lambda_res(2:end),4,1).*nabla_c_res(:, 2:end);
+figure('Position', [0,0, 400. 400])
+ax = subplot(3,1,1);
+plot(t_res(1:end-1), v_res([1,3],:), "LineWidth", 2)
+xlabel("$t$")
+ylabel("$v_x$")
+ylim([40, 75])
+xlim([0, T_final])
+grid on
+ax.FontSize = fontsize;
+ax.FontSize = fontsize;
+ax = subplot(3,1,2);
+plot(t_res(1:end-1), v_res([2,4],:), "LineWidth", 2)
+xlabel("$t$")
+ylabel("$v_y$")
+ax.FontSize = fontsize;
+ax.FontSize = fontsize;
+ylim([40, 75])
+xlim([0, T_final])
+grid on
+ax = subplot(3,1,3);
+plot(t_res, c_res, "LineWidth", 2)
+xlabel("$t$")
+ylabel("$c(x)$")
+ax.FontSize = fontsize;
+ax.FontSize = fontsize;
+ylim([-0.1, 10])
+xlim([0, T_final])
+grid on

examples/new_design_examples/new_design_pds.m

@@ -0,0 +1,52 @@
+
+clear all
+clc
+close all
+import casadi.*
+%% discretization parameters
+N_sim = 31;
+T_sim = 10;
+
+%% NOSNOC settings
+problem_options = nosnoc.Options();
+solver_options = nosnoc.solver.Options();
+problem_options.n_s = 4;
+%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE;
+%problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC;
+problem_options.rk_scheme = RKSchemes.RADAU_IIA;
+%problem_options.rk_representation= RKRepresentation.differential_lift_x; 
+problem_options.rk_representation = RKRepresentation.integral;
+problem_options.cross_comp_mode = CrossCompMode.STAGE_STAGE;
+problem_options.cross_comp_mode = CrossCompMode.FE_FE;
+problem_options.N_sim = N_sim;
+problem_options.N_finite_elements = 2;
+problem_options.T_sim = T_sim;
+problem_options.rho_h = 1e-4;
+%solver_options.homotopy_steering_strategy = 'ELL_INF';
+solver_options.complementarity_tol = 1e-12;
+solver_options.print_level = 2;
+
+model = nosnoc.model.Pds();
+
+model.x0 = [sqrt(2)/2;sqrt(2)/2];
+x = SX.sym('x',2);
+model.x = x;
+model.c = x(2)+0.2;
+model.f_x = [x(2);-x(1)];
+
+model.x0 = [0;pi-2];
+
+integrator = nosnoc.Integrator(model, problem_options, solver_options);
+[t_grid, x_res] = integrator.simulate();
+%
+figure
+plot(x_res(1,:), x_res(2,:))
+grid on
+xlabel('$t$','Interpreter','latex')
+ylabel('$x(t)$','Interpreter','latex')
+grid on
+
+c_fun = casadi.Function('c', {model.x}, {model.c});
+c = full(c_fun(integrator.get_full('x')))';
+x_full = integrator.get_full('x');
+lam_full = integrator.get_full('lambda');