Constrained pendulum problem

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

@@ -0,0 +1,22 @@
+classdef Canonical < otp.constrainedpendulum.ConstrainedPendulumProblem
+    %CANONICAL The constrained pendulum problem
+    %
+    % See
+    %    Ascher, Uri. "On symmetric schemes and differential-algebraic equations."
+    %    SIAM journal on scientific and statistical computing 10.5 (1989): 937-949.
+
+    methods
+        function obj = Canonical
+            tspan = [0; 10];
+
+            params = otp.constrainedpendulum.ConstrainedPendulumParameters;
+            params.Mass = 1;
+            params.Length = 1;
+            params.Gravity = otp.utils.PhysicalConstants.EarthGravity;
+
+            y0 = [sqrt(2)/2; sqrt(2)/2; 0; 0];
+
+            obj = [email protected](tspan, y0, params);
+        end
+    end
+end

src/+otp/+constrainedpendulum/ConstrainedPendulumParameters.m

@@ -0,0 +1,11 @@
+classdef ConstrainedPendulumParameters
+    %CONSTRAINEDPENDULUMPARAMETERS
+    properties
+        %Gravity defines the magnitude of acceleration towards the ground
+        Gravity %MATLAB ONLY: (1,1) {mustBeNonnegative}
+        %Mass defines the mass of the pendulum
+        Mass %MATLAB ONLY: (1,1) {mustBeNonnegative}
+        %Length defines the length of the pendulum
+        Length %MATLAB ONLY: (1,1) {mustBeNonnegative}
+    end
+end

src/+otp/+constrainedpendulum/ConstrainedPendulumProblem.m

@@ -0,0 +1,35 @@
+classdef ConstrainedPendulumProblem < otp.Problem
+    %CONSTRAINEDPENDULUM PROBLEM This is a Hessenberg Index-2 DAE problem posed in terms of three constraints
+    %
+
+    properties
+        Constraints
+        ConstraintsJacobian
+        Z0 = [0; 0; 0];
+    end
+
+    methods
+        function obj = ConstrainedPendulumProblem(timeSpan, y0, parameters)
+            [email protected]('Constrained Pendulum', 4, 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.constrainedpendulum.constraints([], obj.Y0, g, m, l, 0);
+            E0 = initialconstraints(3);
+
+            obj.RHS = otp.RHS(@(t, y) otp.constrainedpendulum.f(t, y, g, m, l, E0));
+
+            obj.Constraints = @(t, y) otp.constrainedpendulum.constraints(t, y, g, m, l, E0);
+            obj.ConstraintsJacobian = @(t, y) otp.constrainedpendulum.constraintsjacobian(t, y, g, m, l, E0);
+
+        end
+    end
+end
+

src/+otp/+constrainedpendulum/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/+constrainedpendulum/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/+constrainedpendulum/f.m

@@ -0,0 +1,19 @@
+function dstate = f(~, state, g, m, l, ~)
+
+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