[WIP] New design CLS Pipeline

+nosnoc/+model/Base.m

@@ -342,20 +342,20 @@ function verify_and_backfill(obj, opts)
             if size(obj.g_path, 1) ~= 0
                 g_path_constraint  = 1;
                 dims.n_g_path = length(obj.g_path);
-                if size(obj.g_path_lb, 1) ~= 0
-                    if length(obj.g_path_lb)~=dims.n_g_path;
-                        error('The user provided vector g_path_lb has the wrong size.')
+                if size(obj.lbg_path, 1) ~= 0
+                    if length(obj.lbg_path)~=dims.n_g_path;
+                        error('The user provided vector lbg_path has the wrong size.')
                     end
                 else
-                    obj.g_path_lb = -inf*ones(dims.n_g_path,1);
+                    obj.lbg_path = -inf*ones(dims.n_g_path,1);
                 end
 
-                if size(obj.g_path_ub, 1) ~= 0
-                    if length(obj.g_path_ub)~=dims.n_g_path;
-                        error('The user provided vector g_path_ub has the wrong size.')
+                if size(obj.ubg_path, 1) ~= 0
+                    if length(obj.ubg_path)~=dims.n_g_path;
+                        error('The user provided vector ubg_path has the wrong size.')
                     end
                 else
-                    obj.g_path_ub =  0*ones(dims.n_g_path,1);
+                    obj.ubg_path =  0*ones(dims.n_g_path,1);
                 end
             else
                 dims.n_g_path = 0;
@@ -408,12 +408,12 @@ function verify_and_backfill(obj, opts)
 
                 % some custom handling for objective functions:
                 if strcmp(name, 'f_q')
-                    if obj.f_q == 0
+                    if class(obj.f_q) ~= "casadi.SX" && obj.f_q == 0
                         continue
                     end
                 end
                 if strcmp(name, 'f_q_T')
-                    if obj.f_q_T == 0
+                    if class(obj.f_q_T) ~= "casadi.SX" && obj.f_q_T == 0
                         continue
                     end
                 end

+nosnoc/+model/Cls.m

@@ -0,0 +1,179 @@
+classdef Cls < nosnoc.model.Base
+% A class of systems of rigid bodies with contacts and friction with model equations:
+%
+% .. math::
+%     :nowrap:
+%
+%     \begin{align*}
+%        \dot{q}       &= v, \\
+%        M\dot{v}     &= f_v(q,v) + \sum_{i=1}^{n_c} J_n^i \lambda_n^i + J_t^i \lambda_t^i, \\
+%        0            &\le \lambda_n^i \perp f_c^i(q) \ge 0\\
+%        0            &= J_n^i(q(t_s))^\top (v(t_s^+) + ev(t_s^-))\quad \mathrm{if}\ f_c^i(t_s) = 0\ \mathrm{and}\ J_n^i(q(t_s))^\top v(t_s^-) < 0 \\
+%        \lambda_t^i  &\in \operatorname{arg\, min}_{\lambda_t^i\in\mathbb{R}^{n_t}} -v^\top  J_t^i \lambda_t^i\quad \mathrm{s.t.}\quad ||\lambda_t^i||_2 \le \mu^i \lambda_n^i
+%     \end{align*}
+%
+% With $i = 1\ldots n_c$.
+%
+% See Also:
+%     More information about this model and its discretization can be found in :cite:`Nurkanovic2024`.
+    properties
+        q % casadi.SX|casadi.MX: Gemeralized coordinates $q\in\mathbb{R}^{n_q}$.
+        v % casadi.SX|casadi.MX: Gemeralized velocities $v\in\mathbb{R}^{n_q}$.
+        f_v % casadi.SX|casadi.MX: Generalized acceleration $\dot{v} = f_v(x)\in\mathbb{R}^{n_q}$.
+        f_c % casadi.SX|casadi.MX: Contact gap functions $f_c(q)\in\mathbb{R}^{n_c}$.
+
+        % double: Friction coefficients $0\le\mu$. If a scalar value is passed then it is assumed all
+        % contacts have the same friction coefficient. Otherwise this needs to be a vector of size $n_c$.
+        mu
+
+        % double: Coefficient of restituition $0\le e \le 1$. If a scalar value is passed then it is
+        % assumed that all contact have the same coefficient of restitution. Otherwise this needs to
+        % be a vector of size $n_c$.
+        e
+
+        M % casadi.SX|casadi.MX|double: Generalized inertia matrix. Can be a function of the state $q$.
+        invM % casadi.SX|casadi.MX|double: Inverse of the generalized inertial matrix. TODO(@anton) should this be private/readonly
+
+        friction_exists % boolean: Set to true if any $\mu \neq 0$.
+
+        J_normal % casadi.SX|casadi.MX: Normal contact Jacobian $J_n$. This can be calculated automatically from the contact gap functions.
+        J_tangent % casadi.SX|casadi.MX: Tangent contact Jacobian $J_t$. This must be provided if there is friction and using the Conic :attr:`~nosnoc.Options.friction_model`.
+
+        % casadi.SX|casadi.MX: Tangent contact polyherdal approximation.
+        % This must be provided if there is friction and using the Polyhedral :mat:attr:`~nosnoc.Options.friction_model`.
+        % For every row $D_i$, $-D_i$ must also be a row in $D$.
+        D_tangent 
+    end
+
+    methods
+        function obj = Cls()
+            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;
+
+            % TODO(@anton) generalizing this is a massive pain. Choose a strict interface.
+            if isempty(obj.q)
+                obj.q = obj.x(1:(dims.n_x/2));
+            end
+
+            if isempty(obj.v)
+                obj.v = obj.x((dims.n_x/2 + 1):end);
+            end
+
+            dims.n_q = dims.n_x/2;
+            dims.n_v = dims.n_x/2;
+
+            if size(obj.f_v,1) ~= dims.n_v
+                error("nosnoc: f_v has incorrect dimension. It must have the same dimension as x.")
+            end
+
+            dims.n_c = size(obj.f_c,1);
+
+            if isempty(obj.e)
+                error("nosnoc: Please provide a coefficient of restitution via model.e")
+            else
+                if length(obj.e) ~= 1 && length(obj.e) ~= dims.n_c
+                    error('The length of model.e has to be one or match the length of model.f_c')
+                end
+                if length(obj.e) == 1
+                    obj.e = obj.e*ones(dims.n_c,1);
+                end
+            end
+            if any(abs(1-obj.e)>1) || any(obj.e<0)
+                error('nosnoc: the coefficient of restitution e should be in [0,1].')
+            end
+
+            % coefficient of friction checks
+            if size(obj.mu, 1) ~= 0
+                if length(obj.mu) ~= 1 && length(obj.mu) ~= dims.n_c
+                    error('The length of model.mu has to be one or match the length of model.f_c')
+                end
+                if length(obj.mu) == 1
+                    obj.mu = obj.mu*ones(dims.n_c,1);
+                end
+
+                if any(obj.mu > 0)
+                    obj.friction_exists = 1;
+                else
+                    obj.friction_exists = 0;
+                end
+            else
+                obj.mu = zeros(dims.n_c,1);
+                fprintf('nosnoc: Coefficients of friction mu not provided, setting it to zero for all contacts. \n')
+            end
+            if any(obj.mu<0)
+                error('nosnoc: The coefficients of friction mu should be nonnegative.')
+            end
+
+            if isempty(obj.M)
+                fprintf('nosnoc: Inertia matrix M is not provided, set to default value: M = eye(n_q). \n')
+                obj.M = eye(dims.n_q);
+                obj.invM = inv(obj.M);
+            elseif any(size(obj.M) ~= dims.n_q)
+                error('nosnoc: Inertia matrix has incorrect dimensions.')
+            else
+                obj.invM = inv(obj.M);
+            end
+
+            if size(obj.J_normal, 1) ~= 0
+                J_normal_exists = 1;
+            else
+                J_normal_exists = 0;
+            end
+
+            if J_normal_exists
+                if size(obj.J_normal,1)~=dims.n_q && size(obj.J_normal,2)~=dims.n_c
+                    fprintf('nosnoc: J_normal should be %d x %d matrix.\n',dims.n_q,dims.n_c);
+                    error('nosnoc: J_normal has the wrong size.')
+                end
+                J_normal_exists = 1;
+            else
+                obj.J_normal = obj.f_c.jacobian(obj.q)';
+                fprintf('nosnoc: normal contact Jacobian not provided, but it is computed from the gap functions.\n');
+                J_normal_exists = 1;
+            end
+
+            % Tangent Contact Jacobian
+            if obj.friction_exists
+                if isequal(opts.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.')
+                        end
+                    else
+                        error('nosnoc: please provide the tangent Jacobian in model.J_tangent.')
+                    end
+                end
+
+                if isequal(opts.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
+                end
+            end
+            % Dimension of tangents
+            dims.n_t = 0;
+            if obj.friction_exists
+                if isequal(opts.friction_model,'Polyhedral')
+                    dims.n_t = size(obj.D_tangent,2)/dims.n_c; % number of tangent multipliers for a single contact
+                elseif isequal(opts.friction_model,'Conic')
+                    dims.n_t = size(obj.J_tangent,2)/dims.n_c; % number of tangent multipliers for a single contact
+                end
+                dims.n_tangents = dims.n_t*dims.n_c; % number tangent forces for all multpliers
+            else
+                dims.n_tangents = 0;
+            end
+
+            obj.dims = dims;
+        end
+    end
+end

+nosnoc/+ocp/Solver.m

@@ -44,7 +44,11 @@
                 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")
+                obj.dcs = nosnoc.dcs.Cls(model);
+                obj.dcs.generate_variables(opts);
+                obj.dcs.generate_equations(opts);
+                obj.discrete_time_problem = nosnoc.discrete_time_problem.Cls(obj.dcs, opts);
+                obj.discrete_time_problem.populate_problem();
               case "nosnoc.model.Pds"
                 if ~opts.right_boundary_point_explicit
                     error("You are using an rk scheme with its right boundary point (c_n) not equal to one. Please choose another scheme e.g. RADAU_IIA")

+nosnoc/+solver/MpccSolver.m

@@ -94,6 +94,10 @@
                   case HomotopySteeringStrategy.ELL_INF
                     % adding a scalar elastic variable to nlp.w which augments the original mpcc.w
                     nlp.w.s_elastic = {{'s_elastic', 1}, opts.s_elastic_min, opts.s_elastic_max, opts.s_elastic_0};
+                    if opts.decreasing_s_elastic_upper_bound
+                        nlp.g.s_ub = {nlp.w.s_elastic() - nlp.p.sigma_p(), -inf, 0};
+                    end
+
                     sigma = nlp.w.s_elastic(); % Here s_elastic takes the role of sigma in direct, and sigma_p is used to define a penalty parameter for the elastic variable s_elastic
                     if opts.objective_scaling_direct
                         nlp.f = nlp.f + (1/nlp.p.sigma_p())*sigma; % penalize the elastic more and more with decreasing sigma_p
@@ -105,6 +109,10 @@
                     % Remark: ELL_1 with s_elastic is equivalent to usually Ell_1 penality approach, but this indirect way helps
                     % to add some constraint on s_elastic (which  avoids unbounded problems somtimes, and it can also improve convergence)
                     nlp.w.s_elastic = {{'s_elastic', n_c}, opts.s_elastic_min, opts.s_elastic_max, opts.s_elastic_0};
+                    if opts.decreasing_s_elastic_upper_bound
+                        nlp.g.s_ub = {nlp.w.s_elastic() - nlp.p.sigma_p(), -inf, 0};
+                    end
+
                     sigma = nlp.w.s_elastic();
                     sum_elastic = sum1(sigma);
                     if opts.objective_scaling_direct
@@ -176,6 +184,11 @@
                     obj.plugin = nosnoc.solver.plugins.Uno();
                 end
 
+                % TODO figure out how to get mpcc in here without the horrible hack in the case of vdx mpcc passed in
+                if ~isempty(opts.ipopt_callback)
+                    opts.opts_casadi_nlp.iteration_callback = NosnocIpoptCallback('ipopt_callback', [], nlp, opts, length(nlp.w.sym), length(nlp.g.sym), length(nlp.p.sym));
+                end
+
                 % Construct solver
                 obj.plugin.construct_solver(nlp, opts);
                 obj.nlp = nlp;
@@ -765,6 +778,11 @@
             last_iter_failed = 0;
             timeout = 0;
 
+            % reset s_elastic
+            if opts.homotopy_steering_strategy ~= 'DIRECT'
+                obj.nlp.w.s_elastic().init = opts.s_elastic_0;
+            end
+
             if opts.print_level >= 3
                 plugin.print_nlp_iter_header();
             end

+nosnoc/Integrator.m

@@ -7,6 +7,8 @@
         dcs
         discrete_time_problem
         stats
+
+        solver_exists = false;
     end
 
     properties(Access=private)
@@ -55,7 +57,11 @@
                 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")
+                obj.dcs = nosnoc.dcs.Cls(model);
+                obj.dcs.generate_variables(opts);
+                obj.dcs.generate_equations(opts);
+                obj.discrete_time_problem = nosnoc.discrete_time_problem.Cls(obj.dcs, opts);
+                obj.discrete_time_problem.populate_problem();
               case "nosnoc.model.Pds"
                 if ~opts.right_boundary_point_explicit
                     error("You are using an rk scheme with its right boundary point (c_n) not equal to one. Please choose another scheme e.g. RADAU_IIA")
@@ -75,7 +81,13 @@
             if ~exist('plugin', 'var')
                 plugin = 'scholtes_ineq';
             end
-            obj.discrete_time_problem.create_solver(obj.solver_opts, plugin);
+            if ~obj.solver_exists
+                obj.discrete_time_problem.create_solver(obj.solver_opts, plugin);
+                if isfield(obj.solver_opts.opts_casadi_nlp,'iteration_callback')
+                    obj.solver_opts.opts_casadi_nlp.iteration_callback.mpcc = obj.discrete_time_problem;
+                end
+                obj.solver_exists = true;
+            end
 
             stats = obj.discrete_time_problem.solve();
             obj.stats = [obj.stats,stats];
@@ -94,7 +106,8 @@
             obj.w_all = []; % Clear simulation data.
             opj.stats = []; % Clear simulation stats.
             t_current = 0;
-
+            w0 = obj.discrete_time_problem.w.init; 
+
             for ii=1:opts.N_sim
                 solver_stats = obj.solve(plugin);
                 t_current = t_current + opts.T;
@@ -105,22 +118,39 @@
                         ii, opts.N_sim, t_current, opts.T_sim, solver_stats.cpu_time_total);
                 end
                 obj.w_all = [obj.w_all,obj.discrete_time_problem.w.res];
-                x_step = obj.discrete_time_problem.w.x(:,:,opts.n_s).res;
-                x_step_full = obj.discrete_time_problem.w.x(:,:,:).res;
-                x_res = [x_res, x_step(:,2:end)];
-                x_res_full = [x_res_full, x_step_full(:,2:end)];
-                if opts.use_fesd
-                    h = obj.discrete_time_problem.w.h(:,:).res;
+
+                if obj.opts.dcs_mode ~= DcsMode.CLS
+                    x_step = obj.discrete_time_problem.w.x(:,:,opts.n_s).res;
+                    x_step_full = obj.discrete_time_problem.w.x(:,:,:).res;
+                    x_res = [x_res, x_step(:,2:end)];
+                    x_res_full = [x_res_full, x_step_full(:,2:end)];
+                    if opts.use_fesd
+                        h = obj.discrete_time_problem.w.h(:,:).res;
+                    else
+                        h = ones(1,opts.N_finite_elements) * obj.discrete_time_problem.p.T().val/opts.N_finite_elements;
+                    end
+                    t_grid = [t_grid, t_grid(end) + cumsum(h)];
+                    for ii = 1:length(h)
+                        for jj = 1:opts.n_s
+                            t_grid_full = [t_grid_full; t_grid_full(end) + opts.c_rk(jj)*h(ii)];
+                        end
+                    end
                 else
-                    h = ones(1,opts.N_finite_elements) * obj.discrete_time_problem.p.T().val/opts.N_finite_elements;
-                end
-                t_grid = [t_grid, t_grid(end) + cumsum(h)];
-                for ii = 1:length(h)
-                    for jj = 1:opts.n_s
-                        t_grid_full = [t_grid_full; t_grid_full(end) + opts.c_rk(jj)*h(ii)];
+                    x_step = obj.discrete_time_problem.w.x(:,:,[0,opts.n_s]).res;
+                    x_step_full = obj.discrete_time_problem.w.x(:,:,:).res;
+                    x_res = [x_res, x_step(:,(2+opts.no_initial_impacts):end)];
+                    x_res_full = [x_res_full, x_step_full(:,(2+opts.no_initial_impacts):end)];
+                    h = obj.discrete_time_problem.w.h(:,:).res;
+                    for ii=1:length(h)
+                        hi = h(ii);
+                        if opts.no_initial_impacts && ii==1 
+                            t_grid = [t_grid, t_grid(end)+hi];
+                        else
+                            t_grid = [t_grid, t_grid(end), t_grid(end)+hi];
+                        end
                     end
+                    % TODO(@anton) do full grid
                 end
-
                 if opts.use_previous_solution_as_initial_guess
                     obj.discrete_time_problem.w.init = obj.discrete_time_problem.w.res;
                 end

docs/requirements.txt

@@ -1,5 +1,5 @@
 sphinx==7.3.7
-sphinx-rtd-theme==1.3.0rc1
 furo==2024.05.06
 sphinxcontrib-matlabdomain @ git+https://github.com/apozharski/matlabdomain@master
 sphinx-math-dollar
+sphinxcontrib-bibtex==2.6.2