Constrained pendulum problem

src/+otp/+pendulumdae/+presets/Canonical.m

@@ -0,0 +1,24 @@
+classdef Canonical < otp.pendulumdae.PendulumDAEProblem
+    %CANONICAL The constrained pendulum problem
+    %
+    % See
+    %    Hairer, E., Roche, M., Lubich, C. (1989). Description of differential-algebraic problems. 
+    %    In: The Numerical Solution of Differential-Algebraic Systems by Runge-Kutta Methods. 
+    %    Lecture Notes in Mathematics, vol 1409. Springer, Berlin, Heidelberg. 
+    %    https://doi.org/10.1007/BFb0093948
+
+    methods
+        function obj = Canonical
+            tspan = [0; 10];
+
+            params = otp.pendulumdae.PendulumDAEParameters;
+            params.Mass = 1;
+            params.Length = 1;
+            params.Gravity = otp.utils.PhysicalConstants.EarthGravity;
+
+            y0 = [sqrt(2)/2; sqrt(2)/2; 0; 0; 0; 0; 0];
+
+            obj = [email protected](tspan, y0, params);
+        end
+    end
+end

src/+otp/+pendulumdae/PendulumDAEParameters.m

@@ -0,0 +1,11 @@
+classdef PendulumDAEParameters
+    %CONSTRAINEDPENDULUMPARAMETERS
+    properties
+        %GRAVITY is acceleration due to gravity
+        Gravity %MATLAB ONLY: (1,1) {mustBeReal, mustBeFinite, mustBePositive} = 9.8
+        %MASSE of the node of the pendulum
+        Mass %MATLAB ONLY: (:,1) {mustBeReal, mustBeFinite, mustBePositive} = 1
+        %LENGTH to the node of the pendulum
+        Length %MATLAB ONLY: (:,1) {mustBeReal, mustBeFinite, mustBePositive} = 1
+    end
+end

src/+otp/+pendulumdae/PendulumDAEProblem.m

@@ -0,0 +1,102 @@
+classdef PendulumDAEProblem < otp.Problem
+    %CONSTRAINEDPENDULUM PROBLEM This is a Hessenberg Index-2 DAE problem posed in terms of three constraints
+    %
+
+    properties (SetAccess=protected)
+        RHSDifferential
+        RHSAlgebraic
+        RHSHalfExplicit
+    end
+
+    properties (Dependent)
+      Y0Differential
+      Y0Algebraic
+    end
+
+    methods
+        function obj = PendulumDAEProblem(timeSpan, y0, parameters)
+            [email protected]('Constrained Pendulum', 7, timeSpan, y0, parameters);
+        end
+    end
+
+    methods (Access = protected)
+        function onSettingsChanged(obj)
+            m = obj.Parameters.Mass;
+            l = obj.Parameters.Length;
+            g = obj.Parameters.Gravity;
+
+            % get initial energy
+            initialconstraints = otp.pendulumdae.fAlgebraic([], obj.Y0Differential, g, m, l, 0);
+            E0 = initialconstraints(3);
+
+            % The right hand size in terms of x, y, x', y', and three control parameters z
+            obj.RHS = otp.RHS(@(t, y) otp.pendulumdae.f(t, y, g, m, l, E0), ...
+                'Mass', otp.pendulumdae.mass([], [], g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobian(t, y, g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProduct(t, y, v, g, m, l, E0), ...
+                'Vectorized', 'on');
+
+            % Generate the constituent RHS for the differential part
+            obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fDifferential(t, y, g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobianDifferential(t, y, g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductDifferential(t, y, v, g, m, l, E0), ...
+                'Vectorized', 'on');
+
+            % Generate the constituent RHS for the algebraic part
+            obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.fAlgebraic(t, y, g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobianAlgebraic(t, y, g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductAlgebraic(t, y, v, g, m, l, E0), ...
+                'JacobianAdjointVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, y, v, g, m, l, E0, v), ...
+                'HessianAdjointVectorProduct', ...
+                @(t, y, v, u) otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, y, v, u, g, m, l, E0), ...
+                'Vectorized', 'on');
+
+            % Generate an Auxillary RHS for the half-explicit Jacobian
+            obj.RHSHalfExplicit = otp.RHS([], ...
+                'Jacobian', @(t, y) otp.pendulumdae.jacobianHalfExplicit(t, y, g, m, l, E0), ...
+                'JacobianVectorProduct', ...
+                @(t, y, v) otp.pendulumdae.jacobianVectorProductHalfExplicit(t, y, v, g, m, l, E0), ...
+                'Vectorized', 'on');
+
+        end
+
+        function label = internalIndex2label(~, index)
+            switch index
+                case 1
+                    label = 'x position';
+                case 2
+                    label = 'y position';
+                case 3
+                    label = 'x velocity';
+                case 4
+                    label = 'y velocity';
+                case 5
+                    label = 'Control 1';
+                case 6
+                    label = 'Control 2';
+                case 7
+                    label = 'Control 3';
+            end
+        end
+
+        function sol = internalSolve(obj, varargin)
+            sol = [email protected](obj, 'Solver', @otp.utils.solvers.dae34, varargin{:});
+        end
+    end
+
+    methods
+        function y0differential = get.Y0Differential(obj)
+            y0differential = obj.Y0(1:4);
+        end
+
+        function y0algebraic = get.Y0Algebraic(obj)
+            y0algebraic = obj.Y0(5:end);
+        end
+    end
+
+end
+

src/+otp/+pendulumdae/f.m

@@ -0,0 +1,13 @@
+function dfull = f(t, statepluscontrol, g, m, l, E0)
+
+state   = statepluscontrol(1:4, :);
+control = statepluscontrol(5:end, :);
+
+dstate = otp.pendulumdae.fDifferential(t, state, g, m, l, E0) ...
+    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, control, g, m, l, E0);
+
+c = otp.pendulumdae.fAlgebraic(t, state, g, m, l, E0);
+
+dfull = [dstate; c];
+
+end

src/+otp/+pendulumdae/fAlgebraic.m

@@ -0,0 +1,19 @@
+function c = fAlgebraic(~, state, g, m, l, E0)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+% the fisrt invariant is length preservation
+c1 = x.^2 + y.^2 - l^2;
+
+% the second invariant is the derivative of the above, to convert this into an index-2 DAE
+c2 = 2*(x.*u + y.*v);
+
+% the third invariant is energy preservation, for numerical stability
+c3 = m*(g*(y + l) + 0.5*(u.^2 + v.^2)) - E0;
+
+c = [c1; c2; c3];
+
+end

src/+otp/+pendulumdae/fDifferential.m

@@ -0,0 +1,19 @@
+function dstate = fDifferential(~, state, g, m, ~, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+lxy2 = x.^2 + y.^2;
+
+lambda =  (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y;
+
+dx = u;
+dy = v;
+du = -(lambda/m).*x;
+dv = -(lambda/m).*y - g/m;
+
+dstate = [dx; dy; du; dv];
+
+end

src/+otp/+pendulumdae/hessianAdjointVectorProductAlgebraic.m

@@ -0,0 +1,19 @@
+function dvp = hessianAdjointVectorProductAlgebraic(~, ~, control, vec, ~, m, ~, ~)
+
+z1 = control(1, :);
+z2 = control(2, :);
+z3 = control(3, :);
+
+vec1 = vec(1, :);
+vec2 = vec(2, :);
+vec3 = vec(3, :);
+vec4 = vec(4, :);
+
+dvp1 = 2*(vec1.*z1 + vec3.*z2);
+dvp2 = 2*(vec2.*z1 + vec4.*z2);
+dvp3 = 2*vec1.*z2 + m*vec3.*z3;
+dvp4 = 2*vec2.*z2 + m*vec4.*z3;
+
+dvp = [dvp1; dvp2; dvp3; dvp4];
+
+end

src/+otp/+pendulumdae/jacobian.m

@@ -0,0 +1,15 @@
+function j = jacobian(t, statepluscontrol, g, m, l, E0)
+
+state   = statepluscontrol(1:4);
+control = statepluscontrol(5:end);
+
+jaccontrol = otp.pendulumdae.jacobianAlgebraic(t, state, g, m, l, E0);
+
+jacstate = otp.pendulumdae.jacobianDifferential(t, state, g, m, l, E0) ...
+    - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, control, eye(4), g, m, l, E0);
+
+jacstatecontrol = -jaccontrol.';
+
+j = [jacstate, jacstatecontrol; jaccontrol, zeros(3, 3)];
+
+end

src/+otp/+pendulumdae/jacobianAdjointVectorProductAlgebraic.m

@@ -0,0 +1,19 @@
+function dvp = jacobianAdjointVectorProductAlgebraic(~, state, control, g, m, ~, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+z1 = control(1, :);
+z2 = control(2, :);
+z3 = control(3, :);
+
+dvp1 = 2*(x.*z1 + u.*z2);
+dvp2 = 2*(y.*z1 + v.*z2) + g*m*z3;
+dvp3 = 2*x.*z2 + m*u.*z3;
+dvp4 = 2*y.*z2 + m*v.*z3;
+
+dvp = [dvp1; dvp2; dvp3; dvp4];
+
+end

src/+otp/+pendulumdae/jacobianAlgebraic.m

@@ -0,0 +1,27 @@
+function dc = jacobianAlgebraic(~, state, g, m, ~, ~)
+
+x = state(1);
+y = state(2);
+u = state(3);
+v = state(4);
+
+dc1dx = 2*x;
+dc1dy = 2*y;
+dc1du = 0;
+dc1dv = 0;
+
+dc2dx = 2*u;
+dc2dy = 2*v;
+dc2du = 2*x;
+dc2dv = 2*y;
+
+dc3dx = 0;
+dc3dy = m*g;
+dc3du = m*u;
+dc3dv = m*v;
+
+dc = [dc1dx, dc1dy, dc1du, dc1dv; ...
+   dc2dx, dc2dy, dc2du, dc2dv; ...
+   dc3dx, dc3dy, dc3du, dc3dv];
+
+end

src/+otp/+pendulumdae/jacobianDifferential.m

@@ -0,0 +1,24 @@
+function j = jacobianDifferential(~, state, g, m, ~, ~)
+
+x = state(1);
+y = state(2);
+u = state(3);
+v = state(4);
+
+lxy2 = x.^2 + y.^2;
+
+lambda = (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y;
+
+dlambdadx = (-2*m*(u.^2 + v.^2).*x + 2*g*x.*y)./(lxy2.^2);
+dlambdady = (-2*m*(u.^2 + v.^2).*y + g*(y.^2 - x.^2))./(lxy2.^2);
+dlambdadu = (2*m./lxy2).*u;
+dlambdadv = (2*m./lxy2).*v;
+
+dx = [0, 0, 1, 0];
+dy = [0, 0, 0, 1];
+du = -(1/m)*[lambda + x.*dlambdadx, x.*dlambdady, x.*dlambdadu, x.*dlambdadv];
+dv = -(1/m)*[y.*dlambdadx, lambda + y.*dlambdady, y.*dlambdadu, y.*dlambdadv];
+
+j = [dx; dy; du; dv];
+
+end

src/+otp/+pendulumdae/jacobianHalfExplicit.m

@@ -0,0 +1,19 @@
+function gyfz = jacobianHalfExplicit(~, state, g, m, ~, ~)
+
+x = state(1);
+y = state(2);
+u = state(3);
+v = state(4);
+
+gyfz11 = -4*(x^2 + y^2);
+gyfz12 = -4*(u*x + v*y);
+gyfz13 = -2*g*m*y;
+gyfz22 = -4*(x^2 + y^2 + u^2 + v^2);
+gyfz23 = -2*m*(u*x + v*(g + y));
+gyfz33 = -(m^2)*(g^2 + u^2 + v^2);
+
+gyfz = [gyfz11, gyfz12, gyfz13; ...
+    gyfz12, gyfz22, gyfz23; ...
+    gyfz13, gyfz23, gyfz33];
+
+end

src/+otp/+pendulumdae/jacobianVectorProduct.m

@@ -0,0 +1,17 @@
+function dfull = jacobianVectorProduct(t, statepluscontrol, v, g, m, l, E0)
+
+state   = statepluscontrol(1:4, :);
+control = statepluscontrol(5:end, :);
+
+vstate = v(1:4, :);
+vcontrol = v(5:end, :);
+
+dstate = otp.pendulumdae.jacobianVectorProductDifferential(t, state, vstate, g, m, l, E0) ...
+    - otp.pendulumdae.hessianAdjointVectorProductAlgebraic(t, state, control, vstate, g, m, l, E0) ...
+    - otp.pendulumdae.jacobianAdjointVectorProductAlgebraic(t, state, vcontrol, g, m, l, E0);
+
+c = otp.pendulumdae.jacobianVectorProductAlgebraic(t, state, vstate, g, m, l, E0);
+
+dfull = [dstate; c];
+
+end

src/+otp/+pendulumdae/jacobianVectorProductAlgebraic.m

@@ -0,0 +1,20 @@
+function dvp = jacobianVectorProductAlgebraic(~, state, vec, g, m, ~, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+vec1 = vec(1, :);
+vec2 = vec(2, :);
+vec3 = vec(3, :);
+vec4 = vec(4, :);
+
+
+dvp1 = 2*(x.*vec1 + y.*vec2);
+dvp2 = 2*(u.*vec1 + v.*vec2 + x.*vec3 + y.*vec4);
+dvp3 = m*(g*vec2 + u.*vec3 + v.*vec4);
+
+dvp = [dvp1; dvp2; dvp3];
+
+end

src/+otp/+pendulumdae/jacobianVectorProductDifferential.m

@@ -0,0 +1,31 @@
+function dstatevp = jacobianVectorProductDifferential(~, state, vec, g, m, ~, ~)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+vec1 = vec(1, :);
+vec2 = vec(2, :);
+vec3 = vec(3, :);
+vec4 = vec(4, :);
+
+lxy2 = x.^2 + y.^2;
+
+lambda = (m./lxy2).*(u.^2 + v.^2) - (g./lxy2).*y;
+
+dlambdadx = (-2*m*(u.^2 + v.^2).*x + 2*g*x.*y)./(lxy2.^2);
+dlambdady = (-2*m*(u.^2 + v.^2).*y + g*(y.^2 - x.^2))./(lxy2.^2);
+dlambdadu = (2*m./lxy2).*u;
+dlambdadv = (2*m./lxy2).*v;
+
+inner = (vec1.*dlambdadx + vec2.*dlambdady + vec3.*dlambdadu + vec4.*dlambdadv);
+
+dxvp = vec3;
+dyvp = vec4;
+duvp = -(1/m)*(vec1.*lambda + x.*inner);
+dvvp = -(1/m)*(vec2.*lambda + y.*inner);
+
+dstatevp = [dxvp; dyvp; duvp; dvvp];
+
+end