bvp5c:Unable to solve the collocation equations -- a singular Jacobian encountered

Hello MATLAB community,

I hope this message finds you well. I am relatively new to MATLAB and am currently working on solving a two-point boundary value problem using the bvp5c solver.

Unfortunately, I am facing challenges, and I would greatly appreciate any help or guidance.

Project Overview:

I am attempting to reprogram code for the optimal swing-up of a double inverted pendulum using the indirect method.

While the code in the paper "Optimal Swing up of Double Inverted Pendulum using Indirect Method" lacks MATLAB implementation details, I've made an attempt based on the project description.

My Code:

doublePendulumSwingUp()

Error using bvp5c
Unable to solve the collocation equations -- a singular Jacobian encountered

Error in solution>doublePendulumSwingUp (line 25)
sol = bvp5c(@odeFunction, @bcFunction, solinit, options);

function doublePendulumSwingUp
%parameters
J1 = 0.0024;
J2 = 0.0079;
l1 = 0.2;
l2 = 0.2;
a1 = 0.10;
a2 = 0.10;
m0 = 0.5;
m1 = 0.55;
m2 = 0.54;
c1 = 0.005;
c2 = 0.005;
g = 9.81;
% Set up time span
T = 2.2;
tspan = linspace(0, T, 1000);

initialConditions = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];

solinit = bvpinit(tspan, initialConditions);

options = bvpset('RelTol', 1e-6, 'AbsTol', 1e-6);
sol = bvp5c(@odeFunction, @bcFunction, solinit, options);


figure;
plot(sol.x, sol.y);
title('Lösung des BVP-Problems');
xlabel('Time');
ylabel('State');
legend('w', 'phi1', 'phi2', 'dw', 'dphi1', 'dphi2', 'lam1', 'lam2', 'lam3', 'lam4', 'lam5', 'lam6');



    function dydt = odeFunction(t, y)
        w = y(1);
        phi1 = y(2);
        phi2 = y(3);
        dw = y(4);
        dphi1 = y(5);
        dphi2 = y(6);
        lam1= y(7);
        lam2= y(8);
        lam3= y(9);
        lam4= y(10);
        lam5= y(11);
        lam6= y(12);

        D11=m0+m1+m2;
        D12=(m1*a1+m2*l1)*cos(phi1);
        D13=m2*a2*cos(phi2);
        D22=m1*a1^2+m2*l1^2+J1;
        D23=m2*l1*a2*cos(phi1-phi2);
        D33=m2*a2^2+J2;
        b1=-(m1*a1+m2*l1)*dphi2^2*sin(phi1)-m2*a2*dphi2^2*sin(phi2);
        b2=m2*l1*a2*dphi2^2*sin(phi1-phi2)+c1*dphi1-c2*(dphi2-dphi1);
        b3=-m2*l1*a2*dphi1^2*sin(phi1-phi2)+c2*(dphi2-dphi1);
        h1=0;
        h2=(m1*a1+m2*l1)*g*sin(phi1);
        h3=m2*a2*g*sin(phi2);



        lam=[lam1;lam2;lam3;lam4;lam5;lam6];

        u=-(lam4*(D22*D33-D23^2)-lam5*(D12*D33-D13*D23)-lam6*D13*D22-D12*D23)/(D11*D22*D33-D11*D23^2-D12^2*D33+2*D12*D13*D23-D13^2*D22);


        u1=u;
        u2=0;
        u3=0;

        M = [D11, D12, D13;
            0,   D22, D23;
            0,   0,   D33];
        U=[u1;u2;u3];
        b=[b1;b2;b3];
        h=[h1;h2;h3];

        xdot= M \ (U-b+h);

        x1=w;
        x2=phi1;
        x3=phi2;
        x4=dw;
        x5=dphi1;
        x6=dphi2;

        x=[x1;x2;x3;x4;x5;x6];

        %StateSpace Form
        f1=x4;
        f2=x5;
        f3=x6;
        f4=xdot(1);
        f5=xdot(2);
        f6=xdot(3);

        f=[f1;f2;f3;f4;f5;f6];


        H = 0.5*u + lam1*f1 + lam2*f2 + lam3*f3 + lam4*f4 + lam5*f5 + lam6*f6;

        %Co State Equations
        dlam1 = -gradient(H, w);
        dlam2 = -gradient(H, phi1);
        dlam3 = -gradient(H, phi2);
        dlam4 = -gradient(H, dw);
        dlam5 = -gradient(H, dphi1);
        dlam6 = -gradient(H, dphi2);


        dydt = [
            f1;
            f2;
            f3;
            f4;
            f5;
            f6;
            dlam1;
            dlam2;
            dlam3;
            dlam4;
            dlam5;
            dlam6];
    end

    function res = bcFunction(ya, yb)
        res = [
            ya(1);
            ya(2) + pi;
            ya(3) + pi;
            ya(4);
            ya(5);
            ya(6);
            yb(1);
            yb(2);
            yb(3);
            yb(4);
            yb(5);
            yb(6)
            ];
    end

end

[Torsten]

H = 0.5*u + lam1*f1 + lam2*f2 + lam3*f3 + lam4*f4 + lam5*f5 + lam6*f6

always gives a single numerical value for H, not a symbolic expression that could be differentiated using "gradient".

"gradient" of a single numerical value is always 0, thus

%Co State Equations
dlam1 = -gradient(H, w);
dlam2 = -gradient(H, phi1);
dlam3 = -gradient(H, phi2);
dlam4 = -gradient(H, dw);
dlam5 = -gradient(H, dphi1);
dlam6 = -gradient(H, dphi2);

gives 0 for all dlami.

[Jonas]

Ok, i understand what you mean. I´ve already found an error it must be:

H = 0.5*u^2 + lam1*f1 + lam2*f2 + lam3*f3 + lam4*f4 + lam5*f5 + lam6*f6.

But I don´t understand were I did my mistake. And is it right to use gradient or should I better use diff?

[Torsten]

As I said, in your computational flow, H is one single numerical value, not a function of the variables w, phi1, phi2, dw, dphi1 and dphi2.

Thus no operation (gradient,diff,...) can give you derivatives of H with respect to independent variables.

Maybe you could include your equations in a mathematical notation - it's easier to find a solution for your problem than reconstructing it from your code.

Answer 1

[Jonas]

Here its f4,f5 and f6!

Boundary conditions:

In equation 19 I changed x and xdot to w and dw.

I hope this helps, as I said I am not very familiar with Matlab and especially not with the bvp4c solver. I would appreciate any help. The full issue can be found here: https://journals.iau.ir/article_668290_3040ba0d45a450f12b0be63ec45cb4da.pdf

Answer 2

[Torsten]

I get

syms x1 x2 x3 x4 x5 x6 real
syms D11 D12 D13 D22 D23 D33 real
syms u l1 l2 l3 l4 l5 l6 f1 f2 f3 f4 f5 f6 h1 h2 h3 b1 b2 b3 f456 real
D = [D11 D12 D13;D12 D22 D23;D13 D23 D33];
f1 = x4;
f2 = x5;
f3 = x6;
f456 = inv(D)*([u;0;0]-[b1;b2;b3]-[h1;h2;h3]);
f4 = f456(1);
f5 = f456(2);
f6 = f456(3);
df4du = diff(f4,u);
df5du = diff(f5,u);
df6du = diff(f6,u);
solu = solve(u + l4*df4du + l5*df5du + l6*df6du==0,u);
f4 = subs(f4,u,solu);
f5 = subs(f5,u,solu);
f6 = subs(f6,u,solu);
u = solu;
H = 0.5*u^2+l1*f1+l2*f2+l3*f3+l4*f4+l5*f5+l6*f6;
dl1 = -diff(H,x1)
dl2 = -diff(H,x2)
dl3 = -diff(H,x3)
dl4 = -diff(H,x4)
dl5 = -diff(H,x5)
dl6 = -diff(H,x6)
dx1 = f1
dx2 = f2
dx3 = f3
dx4 = f4
dx5 = f5
dx6 = f6

[Jonas]

Is there a specific rationale for calculating 'u' rather than utilizing the equation '24' for 'u'? I've spent the past few hours attempting to implement this, but my efforts seem to result in an endless loop.

[Torsten]

Is there a specific rationale for calculating 'u' rather than utilizing the equation '24' for 'u'?

I wanted to see if I get the same u as in (24) by solving dH/du = 0 for u.

I've spent the past few hours attempting to implement this, but my efforts seem to result in an endless loop.

The easiest way for you as an unexperienced MATLAB user is to use two separate codes. Just copy the symbolic results for dl1,...,dl6,dx1,...,dx6 of the last twelve lines from the above symbolic code in your numerical code for "odeFunction". For compatibility reasons, you should of course use the same variable names in both codes.

If you were more advanced, you could use one single code and use "matlabFunction" to convert the gained symbolic expressions to numerical functions and insert the numerical values for the variables into these functions to get a numerical dydt back.

[Jonas]

Thank you, the bvp5c solver is now operational!

I am now looking to identify the optimal solution for this problem by minimizing a cost function. I'm not particularly familiar with this process, but I believe I should use fmincon. Any assistance would be greatly appreciated!

[Torsten]

Thank you, the bvp5c solver is now operational!

Are you sure ? You get values in the order of 1e15 for u and dw !

[Jonas]

Yes, from what I understand, the solver provides a possible solution but not necessarily the best one. To obtain the optimal solution, a cost function needs to be minimized.

And L=0.5*u^2.

[Torsten]

Yes, from what I understand, the solver provides a possible solution but not necessarily the best one. To obtain the optimal solution, a cost function needs to be minimized.

As far as I know, the problem as stated gives the optimal solution. The cost function has already been integrated in your equations.

[Jonas]

Hmm, okay, I probably need a better initial guess and constraints for the states. Below, I've added my existing code.

function doublePendulumSwingUp
% Parameters
J1 = 0.0024;
J2 = 0.0079;
L1 = 0.2;
L2 = 0.2;
a1 = 0.10;
a2 = 0.10;
m0 = 0.5;
m1 = 0.55;
m2 = 0.54;
c1 = 0.005;
c2 = 0.005;
g = 9.81;

T = 2.2;
tspan = linspace(0, T, 1000);
initialConditions = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
solinit = bvpinit(tspan, initialConditions);
options = bvpset('RelTol', 1e-8, 'AbsTol', 1e-8);
sol = bvp5c(@odeFunction, @bcFunction, solinit, options);

figure;
plot(sol.x, sol.y(1:6, :));
title('Solution of the BVP');
xlabel('Time');
ylabel('State');
legend('dw', 'dphi1', 'phi2','u', 'ddphi1', 'ddphi2');


% ODE function
    function dydt = odeFunction(t, y)
        w = y(1);
        phi1 = y(2);
        phi2 = y(3);
        dw = y(4);
        dphi1 = y(5);
        dphi2 = y(6);
        l1 = y(7);
        l2 = y(8);
        l3 = y(9);
        l4 = y(10);
        l5 = y(11);
        l6 = y(12);

        D11 = m0 + m1 + m2;
        D12 = (m1*a1 + m2*L1)*cos(phi1);
        D13 = m2*a2*cos(phi2);
        D22 = m1*a1^2 + m2*L1^2 + J1;
        D23 = m2*L1*a2*cos(phi1 - phi2);
        D33 = m2*a2^2 + J2;
        b1 = -(m1*a1 + m2*L1)*dphi2^2*sin(phi1) - m2*a2*dphi2^2*sin(phi2);
        b2 = m2*L1*a2*dphi2^2*sin(phi1 - phi2) + c1*dphi1 - c2*(dphi2 - dphi1);
        b3 = -m2*L1*a2*dphi1^2*sin(phi1 - phi2) + c2*(dphi2 - dphi1);
        h1 = 0;
        h2 = (m1*a1 + m2*L1)*g*sin(phi1);
        h3 = m2*a2*g*sin(phi2);

        x1 = w;
        x2 = phi1;
        x3 = phi2;
        x4 = dw;
        x5 = dphi1;
        x6 = dphi2;

        % State-space form
        f1 = x4;
        f2 = x5;
        f3 = x6;
        f4 = ((b3 + h3)*(D12*D23 - D13*D22))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) +...
             ((b2 + h2)*(D13*D23 - D12*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             ((D23^2 - D22*D33)*(b1 + h1 + (l4*(D23^2 - D22*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             (l6*(D12*D23 - D13*D22))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             (l5*(D13*D23 - D12*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33)))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33);
        f5 = ((D13*D23 - D12*D33)*(b1 + h1 + (l4*(D23^2 - D22*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             (l6*(D12*D23 - D13*D22))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             (l5*(D13*D23 - D12*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33)))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) - ...
             ((b2 + h2)*(D13^2 - D11*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) +...
             ((b3 + h3)*(D12*D13 - D11*D23))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33);
        f6 = ((D12*D23 - D13*D22)*(b1 + h1 + (l4*(D23^2 - D22*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             (l6*(D12*D23 - D13*D22))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             (l5*(D13*D23 - D12*D33))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33)))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) -...
             ((b3 + h3)*(D12^2 - D11*D22))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33) +...
             ((b2 + h2)*(D12*D13 - D11*D23))/...
             (D33*D12^2 - 2*D12*D13*D23 + D22*D13^2 + D11*D23^2 - D11*D22*D33);

        f = [f1; f2; f3; f4; f5; f6];

        % Co-state equations
        dlam1 = 0;
        dlam2 = 0;
        dlam3 = 0;
        dlam4 = -l1;
        dlam5 = -l2;
        dlam6 = -l3;

        dydt = [f; dlam1; dlam2; dlam3; dlam4; dlam5; dlam6];
    end

% Boundary conditions
    function res = bcFunction(ya, yb)
        res = [ya(1); ya(2) + pi; ya(3) + pi; ya(4); ya(5); ya(6); yb(1); yb(2); yb(3); yb(4); yb(5); yb(6)];
    end

end

[Torsten]

The boundary conditions you set are not those from the article.

[Jonas]

I changed them but that doesnt make a difference. I am looking for a way to constrain my state variables. What is the best way to limit the values of my state variables? I tried:

lowerBounds = [-2; -5; -5; -8; -10; -10; -inf; -inf; -inf; -inf; -inf; -inf];

upperBounds = [2; 5; 5; 8; 10; 10; inf; inf; inf; inf; inf; inf];

and add these via options into my bvp solver but it does not work.

[Torsten]

The problem you gave is fixed: twelve equations for twelve unknowns together with twelve boundary conditions. There is nothing you could bound here because it should give a unique solution that cannot be influenced by given bounds.

The only thing you can do to help the BVP solver to converge is to supply a good initial guess for the functions. I think the article explains that this has been done when the authors used bvp4c to solve the problem. Read the article.

Did you try to solve the double pendulum problem first without optimal control ? Does this problem give reasonable solution curves ?

[Jonas]

The article describes the utilization of another bvp4c problem for the initial guess. I attempted to implement this approach(https://www.sciencedirect.com/science/article/abs/pii/S0005109806003050 for more details), but encountered again a singular Jacobian issue. When I attempted to set the initial guess using linear interpolations between the boundary conditions on a mesh, an error arose indicating the need for additional ODEs. However, my intention is for the solver to proceed with the next guess, so I probably need some sort of for loop. Below i have added my code, thanks for any help.

function doublePendulumSwingUp
% Parameters
J1 = 0.0024;
J2 = 0.0079;
L1 = 0.2;
L2 = 0.2;
a1 = 0.10;
a2 = 0.10;
m0 = 0.5;
m1 = 0.55;
m2 = 0.54;
c1 = 0.005;
c2 = 0.005;
g = 9.81;

T = 2.2;
%tspan = linspace(0, T, 30);
options = bvpset('RelTol', 1e-8, 'AbsTol', 1e-8);
sol = bvp5c(@odeFunction, @bcFunction, constructInitialGuess(T), options);
%solinit = bvpinit(tspan, [0,0,0,0,0,0,0,0]);
%sol = bvp5c(@odeFunction, @bcFunction, solinit, options);
figure;
plot(sol.x, sol.y(1:4, :));
title('Solution of the BVP');
xlabel('Time');
ylabel('State');
legend('dphi1', 'phi2', 'ddphi1', 'ddphi2');


% ODE function
    function dydt = odeFunction(t, y)
        phi1 = y(1);
        phi2 = y(2);
        dphi1 = y(3);
        dphi2 = y(4);
        p1 = y(5);
        p2 = y(6);
        p3 = y(7);
        p4 = y(8);

        %D11 = m0 + m1 + m2;
        %D12 = (m1*a1 + m2*L1)*cos(phi1);
        %D13 = m2*a2*cos(phi2);
        D22 = m1*a1^2 + m2*L1^2 + J1;
        D23 = m2*L1*a2*cos(phi1 - phi2);
        D33 = m2*a2^2 + J2;
        %b1 = -(m1*a1 + m2*L1)*dphi2^2*sin(phi1) - m2*a2*dphi2^2*sin(phi2);
        b2 = m2*L1*a2*dphi2^2*sin(phi1 - phi2) + c1*dphi1 - c2*(dphi2 - dphi1);
        b3 = -m2*L1*a2*dphi1^2*sin(phi1 - phi2) + c2*(dphi2 - dphi1);
        %h1 = 0;
        h2 = (m1*a1 + m2*L1)*g*sin(phi1);
        h3 = m2*a2*g*sin(phi2);
        g1=b2+h2;
        g2=b3+h3;

        x=-p1-p3-(p2+p4)*cos(pi*t/T)+p1*cos(2*pi*t/T)+p2*cos(3*pi*t/T)+p3*cos(4*pi*t/T)+p4*cos(5*pi*t/T);
        xdot= (pi*sin((pi*t)/T)*(p2 + p4))/T - (2*p1*pi*sin((2*pi*t)/T))/T - (3*p2*pi*sin((3*pi*t)/T))/T - (4*p3*pi*sin((4*pi*t)/T))/T - (5*p4*pi*sin((5*pi*t)/T))/T;
        xdotdot=(pi^2*cos((pi*t)/T)*(p2 + p4))/T^2 - (4*p1*pi^2*cos((2*pi*t)/T))/T^2 - (9*p2*pi^2*cos((3*pi*t)/T))/T^2 - (16*p3*pi^2*cos((4*pi*t)/T))/T^2 - (25*p4*pi^2*cos((5*pi*t)/T))/T^2;
        u=xdotdot;

        %M = [D22, D23;
        %     D23, D33];
        %b = [g1;g2];
        %ydot = M \ b*u;
       
        x1 = phi1;
        x2 = phi2;
        x3 = dphi1;
        x4 = dphi2;

        % State-space form
        f1 = x3;
        f2 = x4;
        f3=-((D23*g2 - D33*g1)*(4*p1*pi^2*cos((2*pi*t)/T) - p2*pi^2*cos((pi*t)/T) - p4*pi^2*cos((pi*t)/T) + 9*p2*pi^2*cos((3*pi*t)/T) + 16*p3*pi^2*cos((4*pi*t)/T) + 25*p4*pi^2*cos((5*pi*t)/T)))/(T^2*(D23^2 - D22*D33));
        f4=((D22*g2 - D23*g1)*(4*p1*pi^2*cos((2*pi*t)/T) - p2*pi^2*cos((pi*t)/T) - p4*pi^2*cos((pi*t)/T) + 9*p2*pi^2*cos((3*pi*t)/T) + 16*p3*pi^2*cos((4*pi*t)/T) + 25*p4*pi^2*cos((5*pi*t)/T)))/(T^2*(D23^2 - D22*D33));
        f = [f1; f2; f3; f4];

        dydt = [f; p1; p2; p3; p4];
    end

% Boundary conditions
    function res = bcFunction(ya, yb)
        res = [ya(1)+pi; ya(2) + pi; ya(3); ya(4); yb(1); yb(2); yb(3); yb(4)];
    end

function solinit = constructInitialGuess(T)
    numPoints = 30;
    tspan = linspace(0, T, numPoints);

    phi1Guess = interp1([0, T], [pi, 0], tspan, 'linear');
    phi2Guess = interp1([0, T], [pi, 0], tspan, 'linear');
    dphi1Guess = interp1([0, T], [0, 0], tspan, 'linear');
    dphi2Guess = interp1([0, T], [0, 0], tspan, 'linear');

    guessArray = [phi1Guess; phi2Guess; dphi1Guess; dphi2Guess; zeros(4, numPoints)];
    solinit = bvpinit(tspan, guessArray);
end
end

[Torsten]

Use

solinit = bvpinit(tspan, @guess);
sol = bvp5c(@odeFunction, @bcFunction, solinit, options);

function uinit = guess(T)
  % Supply a 8x1 vector "uninit" with the initial guesses for [phi1,phi2,dphi1,dphi2,p1,p2,p3,p4] in T.
end

Again I stress that you should try to solve the 4 equations describing the physical process without optimal control first to see whether bvp4c already fails for this "simple" problem.

[Jonas]

I didn´t applied optimal control I just solved the symbolic equations, like below. Probably there is something wrong with the implemantation of p1-p4 in the code above, that makes the singular jacobian error.

syms x1 x2 x3 x4 m1 m2 a1 a2 L1 L2 c1 c2 J1 J2 phi1 phi2 dphi1 dphi2 g real
syms D12 D13 D22 D23 D33 real
syms u f1 f2 f3 f4 h1 h2 h3 b1 b2 b3 f34 p1 p2 p3 p4 t T real
D =   [D22, D23; D23, D33];
g1=b2-h2-D12;
g2=b3-h3-D13;

y=-p1-p3-(p2+p4)*cos(pi*t/T)+p1*cos(2*pi*t/T)+p2*cos(3*pi*t/T)+p3*cos(4*pi*t/T)+p4*cos(5*pi*t/T);
ydot=diff(y,t);
ydotdot=diff(ydot,t);
u=ydotdot

f1 = x3;
f2 = x4;
f34 = inv(D)*([g1; g2]*u);
f3 = f34(1);
f4 = f34(2);

dx1 = f1
dx2 = f2
dx3 = f3
dx4 = f4
        %D11 = m0 + m1 + m2;
        D12 = (m1*a1 + m2*L1)*cos(phi1);
        D13 = m2*a2*cos(phi2);
        D22 = m1*a1^2 + m2*L1^2 + J1;
        D23 = m2*L1*a2*cos(phi1 - phi2);
        D33 = m2*a2^2 + J2;
        %b1 = -(m1*a1 + m2*L1)*dphi2^2*sin(phi1) - m2*a2*dphi2^2*sin(phi2);
        b2 = m2*L1*a2*dphi2^2*sin(phi1 - phi2) + c1*dphi1 - c2*(dphi2 - dphi1);
        b3 = -m2*L1*a2*dphi1^2*sin(phi1 - phi2) + c2*(dphi2 - dphi1);
        %h1 = 0;
        h2 = (m1*a1 + m2*L1)*g*sin(phi1);
        h3 = m2*a2*g*sin(phi2);
        g1=b2-h2-D12;
        g2=b3-h3-D13;

[Torsten]

And where do you solve for x1 - x4 (= phi1,phi2,dphi1,dphi2) ?

[Jonas]

I assume I've already resolved it in the preceding code. The last code I sent was solely intended for computing the correct symbolic equations.

[Torsten]

No, your tried to solve the optimal control problem, but not the double pendulum problem on its own (i.e. only the four equations for x1-x4):

https://en.wikipedia.org/wiki/Double_pendulum

[Jonas]

Okay, something like this? I attempted to replicate the equations shown in the image, but I've encountered an issue – my gravitational term appears to be positive, whereas in the provided file, it should be negative.

syms t real
syms l1 l2 a1 a2 m1 m2 J1 J2 g c1 c2 theta1 theta2 dtheta1 dtheta2 real

phi1 = str2sym('phi1(t)');
dphi1 = diff(phi1, t);
ddphi1 = diff(dphi1, t);
assume(phi1, 'real');
assume(dphi1, 'real');
assume(ddphi1, 'real');

phi2 = str2sym('phi2(t)');
dphi2 = diff(phi2, t);
ddphi2 = diff(dphi2, t);
assume(phi2, 'real');
assume(dphi2, 'real');
assume(ddphi2, 'real');

x1= a1*sin(phi1);
y1= -a1*cos(phi1);
x2= l1*sin(phi1)+a2*sin(phi2);
y2=-l1*cos(phi1)-a2*cos(phi2);
dx1= dphi1*a1*cos(phi1);
dy1= dphi1*a1*sin(phi1);
dx2= dphi1*l1*cos(phi1)+dphi2*a2*cos(phi2);
dy2= dphi1*l1*sin(phi1)+dphi2*a2*sin(phi2);

link1= dx1^2+dy1^2;
link1=simplify (link1);
dv1=sqrt(link1);
link2= dx2^2+dy2^2;
link2=simplify (link2);
dv2=sqrt(link2);
T=1/2*(m1*dv1^2+m2*dv2^2+J1*dphi1^2+J2*dphi2^2);
V=m1*g*y1+m2*g*y2;

L=T-V; 
gl1=diff(diff(L,dphi1),t)-diff(L,phi1);
gl2=diff(diff(L,dphi2),t)-diff(L,phi2);
simplify(gl1)

ans = 

simplify(gl2)

ans = 

%m1*a1^2*diff(phi1(t), t, t) + m2*l1^2*diff(phi1(t), t, t) + a2*m2*cos(phi1(t) - phi2(t))*l1*diff(phi2(t), t, t) + J1*diff(phi1(t), t, t) + a2*m2*sin(phi1(t) - phi2(t))*l1*diff(phi2(t), t)^2 + g*m1*sin(phi1(t))*a1 + g*m2*sin(phi1(t))*l1
%m2*a2^2*diff(phi2(t), t, t) + l1*m2*cos(phi1(t) - phi2(t))*a2*diff(phi1(t), t, t) + J2*diff(phi2(t), t, t) - l1*m2*sin(phi1(t) - phi2(t))*a2*diff(phi1(t), t)^2 + g*m2*sin(phi2(t))*a2

%Added friction therms +c1*dphi1-c2*(dphi2-dphi1) and -c2*(dphi2-dphi1)
%(m1*a1^2+m2*l1^2+J1)*diff(phi1(t), t, t) + a2*m2*cos(phi1(t)-phi2(t))*l1*diff(phi2(t), t, t)+ a2*m2*sin(phi1(t)-phi2(t))*l1*diff(phi2(t), t)^2 + (m1*a1+m2*l1)*g*sin(phi1(t))+c1*dphi1-c2*(dphi2-dphi1)
%l1*m2*cos(phi1(t)-phi2(t))*a2*diff(phi1(t), t, t) + (m2*a2^2+ J2)*diff(phi2(t), t, t) - l1*m2*sin(phi1(t)-phi2(t))*a2*diff(phi1(t), t)^2 + g*m2*sin(phi2(t))*a2-c2*(dphi2-dphi1)

%phi=theta 
M=[(m1*a1^2+m2*l1^2+J1),      a2*m2*l1*cos(theta1-theta2);
    l1*m2*cos(theta1-theta2)*a2, (m2*a2^2+ J2)];

G=[a2*m2*sin(theta1-theta2)*l1*dtheta2^2 + (m1*a1+m2*l1)*g*sin(theta1)+c1*dtheta1-c2*(dtheta2-dtheta1);
    - l1*m2*sin(theta1-theta2)*a2*dtheta1^2 + g*m2*sin(theta2)*a2-c2*(dtheta2-dtheta1)];

xdot=M\-G;
simplify(xdot)

ans = 

x1=dtheta1;
x2=dtheta2;
x3=xdot(1);
x4=xdot(2);

% Parameters
J1 = 0.0024;
J2 = 0.0079;
l1 = 0.2;
l2 = 0.2;
a1 = 0.10;
a2 = 0.10;
m1 = 0.55;
m2 = 0.54;
c1 = 0.01;
c2 = 0.01;
g = 9.81;

ode = @(t, x) doublePendulumODE(t, x, m1, m2, l1, l2, a1, a2, J1, J2, g, c1, c2);
initialConditions = [0; 0; 1; 0];
tspan = [0, 10];
[t, y] = ode45(ode, tspan, initialConditions);

plot(t, y(:, 1), 'r', t, y(:, 2), 'c',t, y(:, 3), 'k', t, y(:, 4), 'm'); 
xlabel('time');
ylabel('angle (rad)');
legend('phi1', 'phi2','dphi1', 'dphi2');

function dx = doublePendulumODE(t, x, m1, m2, l1, l2, a1, a2, J1, J2, g, c1, c2)
    theta1 = x(1);
    theta2 = x(2);
    dtheta1 = x(3);
    dtheta2 = x(4);

    M = [(m1*a1^2 + m2*l1^2 + J1), a2*m2*l1*cos(theta1 - theta2);
         l1*m2*cos(theta1 - theta2)*a2, (m2*a2^2 + J2)];

    G = [a2*m2*sin(theta1 - theta2)*l1*dtheta2^2 + (m1*a1 + m2*l1)*g*sin(theta1) + c1*dtheta1 - c2*(dtheta2 - dtheta1);
         -l1*m2*sin(theta1 - theta2)*a2*dtheta1^2 + g*m2*sin(theta2)*a2 - c2*(dtheta2 - dtheta1)];

    xdot = M\(-G);
    dx = [dtheta1; dtheta2; xdot(1); xdot(2)];
end

[Jonas]

Thanks Thorsten,

With your help, I was able to successfully solve the code. Your support has been invaluable, and I truly appreciate the time and effort you invested in guiding me through the process. However, I must mention that the acceleration is, unfortunately, too high. In the literature, there is a solution to this issue through the optimization problem-solving method using fmincon.

The following problem is described:

I've already looked up the documentation of fmincon, but I could apprechiate any help since I'm very new to Matlab. And thats my code without optimisation:

function doublePendulumSwingUp
J1 = 0.0126;
J2 = 0.0185;
L1 = 0.323;
L2 = 0.48;
a1 = 0.2145;
a2 = 0.223;
m0 = 0.1;
m1 = 0.853;
m2 = 0.51;
c1 = 0.005;
c2 = 0.005;
g = 9.81;
T = 2.2;
dots= 200;

tspan = linspace(0, T, dots);
options = bvpset('RelTol', 1e-5, 'AbsTol', 1e-5);
solinit = bvpinit(tspan, @(t) guess(t, T));
sol = bvp5c(@odeFunction, @bcFunction, solinit, options);

figure;
plot(sol.x, sol.y(1:4, :));
title('Solution of the BVP');
xlabel('Time');
ylabel('State');
legend('phi1', 'phi2', 'dphi1', 'dphi2');
figure;
plot(sol.x, sol.y(1:2, :));
title('Solution of the BVP');
xlabel('Time');
ylabel('State');
legend('phi1', 'phi2', 'dphi1', 'dphi2');

% Plot x, xdot, and u
figure;
t = linspace(0, T, dots);
x = -sol.y(1, :) - sol.y(3, :) - (sol.y(2, :) + sol.y(4, :)) .* cos(pi * t / T) + sol.y(1, :) .* cos(2 * pi * t / T) + sol.y(2, :) .* cos(3 * pi * t / T) + sol.y(3, :) .* cos(4 * pi * t / T) + sol.y(4, :) .* cos(5 * pi * t / T);
xdot = (pi * sin((pi * t) / T) .* (sol.y(2, :) + sol.y(4, :))) / T - (2 * sol.y(1, :) * pi .* sin((2 * pi * t) / T)) / T - (3 * sol.y(2, :) * pi .* sin((3 * pi * t) / T)) / T - (4 * sol.y(3, :) * pi .* sin((4 * pi * t) / T)) / T - (5 * sol.y(4, :) * pi .* sin((5 * pi * t) / T)) / T;
u = (pi^2 * cos((pi * t) / T) .* (sol.y(2, :) + sol.y(4, :))) / T^2 - (4 * sol.y(1, :) * pi^2 .* cos((2 * pi * t) / T)) / T^2 - (9 * sol.y(2, :) * pi^2 .* cos((3 * pi * t) / T)) / T^2 - (16 * sol.y(3, :) * pi^2 .* cos((4 * pi * t) / T)) / T^2 - (25 * sol.y(4, :) * pi^2 .* cos((5 * pi * t) / T)) / T^2;

subplot(3, 1, 1);
plot(t, x);
title('way vs Time');
xlabel('Time');
ylabel('way');

subplot(3, 1, 2);
plot(t, xdot);
title('velocity vs Time');
xlabel('Time');
ylabel('velocity');

subplot(3, 1, 3);
plot(t, u);
title('acceleration vs Time');
xlabel('Time');
ylabel('acceleration');

% ODE function
    function dydt = odeFunction(t, y)
        phi1 = y(1);
        phi2 = y(2);
        dphi1 = y(3);
        dphi2 = y(4);
        p1 = y(5);
        p2 = y(6);
        p3 = y(7);
        p4 = y(8);

        xdotdot=(pi^2*cos((pi*t)/T)*(p2 + p4))/T^2 - (4*p1*pi^2*cos((2*pi*t)/T))/T^2 - (9*p2*pi^2*cos((3*pi*t)/T))/T^2 - (16*p3*pi^2*cos((4*pi*t)/T))/T^2 - (25*p4*pi^2*cos((5*pi*t)/T))/T^2;
        u=xdotdot;

        x1 = phi1;
        x2 = phi2;
        x3 = dphi1;
        x4 = dphi2;

        % State-space form
        f1 = x3;
        f2 = x4;
        f3=(J2*c2*dphi2 - J2*c2*dphi1 - J2*c1*dphi1 - a2^2*c1*dphi1*m2 - a2^2*c2*dphi1*m2 + a2^2*c2*dphi2*m2 - a2^3*dphi2^2*L1*m2^2*sin(phi1 - phi2) + a2^2*L1*m2^2*u*cos(phi1) + a2^2*g*L1*m2^2*sin(phi1) + J2*a1*m1*u*cos(phi1) + J2*a1*g*m1*sin(phi1) + J2*L1*m2*u*cos(phi1) + J2*g*L1*m2*sin(phi1) - a2*c2*dphi1*L1*m2*cos(phi1 - phi2) + a2*c2*dphi2*L1*m2*cos(phi1 - phi2) - a2^2*dphi1^2*L1^2*m2^2*cos(phi1 - phi2)*sin(phi1 - phi2) + a1*a2^2*m1*m2*u*cos(phi1) - a2^2*L1*m2^2*u*cos(phi1 - phi2)*cos(phi2) + a1*a2^2*g*m1*m2*sin(phi1) - a2^2*g*L1*m2^2*cos(phi1 - phi2)*sin(phi2) - J2*a2*dphi2^2*L1*m2*sin(phi1 - phi2))/(m1*a1^2*a2^2*m2 + J2*m1*a1^2 - a2^2*L1^2*m2^2*cos(phi1 - phi2)^2 + a2^2*L1^2*m2^2 + J1*a2^2*m2 + J2*L1^2*m2 + J1*J2);
        f4=(J1*c2*dphi1 - J1*c2*dphi2 + a1^2*c2*dphi1*m1 - a1^2*c2*dphi2*m1 + c2*dphi1*L1^2*m2 - c2*dphi2*L1^2*m2 + a2*dphi1^2*L1^3*m2^2*sin(phi1 - phi2) + a2*L1^2*m2^2*u*cos(phi2) + a2*g*L1^2*m2^2*sin(phi2) + J1*a2*m2*u*cos(phi2) + J1*a2*g*m2*sin(phi2) + a2*c1*dphi1*L1*m2*cos(phi1 - phi2) + a2*c2*dphi1*L1*m2*cos(phi1 - phi2) - a2*c2*dphi2*L1*m2*cos(phi1 - phi2) + a2^2*dphi2^2*L1^2*m2^2*cos(phi1 - phi2)*sin(phi1 - phi2) + a1^2*a2*m1*m2*u*cos(phi2) - a2*L1^2*m2^2*u*cos(phi1 - phi2)*cos(phi1) + a1^2*a2*g*m1*m2*sin(phi2) - a2*g*L1^2*m2^2*cos(phi1 - phi2)*sin(phi1) + J1*a2*dphi1^2*L1*m2*sin(phi1 - phi2) + a1^2*a2*dphi1^2*L1*m1*m2*sin(phi1 - phi2) - a1*a2*L1*m1*m2*u*cos(phi1 - phi2)*cos(phi1) - a1*a2*g*L1*m1*m2*cos(phi1 - phi2)*sin(phi1))/(m1*a1^2*a2^2*m2 + J2*m1*a1^2 - a2^2*L1^2*m2^2*cos(phi1 - phi2)^2 + a2^2*L1^2*m2^2 + J1*a2^2*m2 + J2*L1^2*m2 + J1*J2);
        f = [f1; f2; f3; f4];

        dydt = [f; p1; p2; p3; p4];
    end

% Boundary conditions
    function res = bcFunction(ya, yb)
        res = [ya(1)+pi; ya(2)+pi; ya(3); ya(4); yb(1); yb(2); yb(3); yb(4)];
    end

    function uinit = guess(t, T)
        % Linear interpolation between boundary conditions
        phi1_init = -pi + (pi/T) * t;
        phi2_init = -pi + (pi/T) * t;
        dphi1_init = 0;
        dphi2_init = 0;
        p_init = zeros(1, 4);

        uinit = [phi1_init, phi2_init, dphi1_init, dphi2_init, p_init];
    end

end