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,54 @@
+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
+    end
+
+    properties (Dependent)
+      Y0Differential
+      Z0Algebraic
+    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.constraints([], 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', @(t, y) otp.pendulumdae.mass(t, y, g, m, l, E0));
+
+            obj.RHSDifferential = otp.RHS(@(t, y) otp.pendulumdae.fdifferential(t, y, g, m, l, E0));
+
+            obj.RHSAlgebraic = otp.RHS(@(t, y) otp.pendulumdae.constraints(t, y, g, m, l, E0), ...
+                'Jacobian', @(t, y) otp.pendulumdae.constraintsjacobian(t, y, g, m, l, E0));
+        end
+    end
+
+    methods
+        function y0differential = get.Y0Differential(obj)
+            y0differential = obj.Y0(1:4);
+        end
+
+        function z0algebraic = get.Z0Algebraic(obj)
+            z0algebraic = obj.Y0(5:end);
+        end
+    end
+
+end
+

src/+otp/+pendulumdae/constraints.m

@@ -0,0 +1,14 @@
+function c = constraints(~, state, g, m, l, E0)
+
+x = state(1, :);
+y = state(2, :);
+u = state(3, :);
+v = state(4, :);
+
+c1 = x.^2 + y.^2 - l^2;
+c2 = 2*(x.*u + y.*v);
+c3 = m*(g*(y + l) + 0.5*(u.^2 + v.^2)) - E0;
+
+c = [c1; c2; c3];
+
+end

src/+otp/+pendulumdae/constraintsjacobian.m

@@ -0,0 +1,27 @@
+function dc = constraintsjacobian(~, 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/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.constraintsjacobian(t, state, g, m, l, E0).'*control;
+
+c = otp.pendulumdae.constraints(t, state, g, m, l, E0);
+
+dfull = [dstate; c];
+
+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/mass.m

@@ -0,0 +1,5 @@
+function M = mass(~, ~, ~, ~, ~, ~)
+
+M = [eye(4), zeros(4, 3); zeros(3, 7)];
+
+end