how to solve this equation of motion?

Hi

I appreciate if anyone can help me to underestand if I can solve this EOM like a normal 2DOF system which I use ODE45 for it.

Here is the paper:

https://www.jvejournals.com/article/16409

which:

function yp = func_forced_5DOF(t,y,M,ws,m1,m2,r,kx,ky,ksi,fx,fy,fsi,fd1,fd2,J,j1,j2,L1,L2)


a0 = (1 - ((ws^2)*Ts*Tr*Sig)^2) + ((ws^2)*((Tr + Ts)^2));
a1 = (2*(1 - ((ws^2)*Ts*Tr*Sig))*ws*Ts*Tr*Sig) - (2*(ws*((Tr + Ts)*Tr)));
a2 = Tr^2 + ((ws^2) * (Ts^2) * (Tr^2) *(Sig^2));
 
Te1 = (K*(ws-y(9))) / ((a2*(y(9)^2))+(a1*y(9))+a0);
Te2 = (K*(ws-y(10))) / ((a2*(y(10)^2))+(a1*y(10))+a0);
 
 

yp(1,:) = y(6,:);
yp(2,:) = y(7,:);
yp(3,:) = y(8,:);
yp(4,:) = y(9,:);
yp(5,:) = y(10,:);
yp(6,:) = ((m1*r*( ((y(9,:)^2) * cos(y(4,:))) + (yp(9,:)*sin(y(4,:))) )) - ( m2*r(  ((y(10,:)^2) * cos(y(5,:))) + (yp(10,:)*sin(y(5,:))) ))  - (fx*y(6,:)) - (kx*y(1,:))) / M;

yp(7,:) = ((m1*r*( ((y(9,:)^2) * sin(y(4,:))) - (yp(9,:)*cos(y(4,:))) )) + ( m2*r(  ((y(10,:)^2) * sin(y(5,:))) - (yp(10,:)*cos(y(5,:))) ))  - (fy*y(7,:)) - (ky*y(2,:))) / M;

yp(8,:) = ((-1*m1*r*L1*( ((y(9,:)^2) * sin(y(4,:))) - (yp(9,:)*cos(y(4,:))) )) + ( m2*r*L2*(  ((y(10,:)^2) * sin(y(5,:))) - (yp(10,:)*cos(y(5,:))) ))  - (fsi*y(8,:)) - (ksi*y(3,:))) / J;

yp(9,:) = (Te1 - (m1*r*( ( yp(7,:) * cos(y(4,:))) - (yp(6,:)*sin(y(4,:)) ))) - ( L1*(  (yp(8,:) * cos(y(4,:))))) - (fd1*y(9,:)) ) / j1;

yp(10,:) = (Te2 - (m2*r*( ( yp(7,:) * cos(y(5,:))) - (yp(6,:)*sin(y(5,:)) ))) + ( L2*(  (yp(8,:) * cos(y(5,:))))) - (fd2*y(10,:)) ) / j2;

Answer 1

[darova]

Here is an idea:

function main
% define constants
m1 = ...
m2 = ...
    function dy = myode(t,u)
        x = u(1);
        dx = u(2);
        %% ...
        phi2 = u(9);
        dphi2 = u(10);
        
        % coefficient matrix 
        % d2x d2y d2psi d2phi1 d2phi2
        A = [M 0 0 -m1*r*sin(phi1) m2*r*sin(phi2)
             0 M 0  m1*r*cos(phi1) m2*r*cos(phi2)
             0 0 J  m1*r*L1*cos(phi1) ...
             % ...
            ];
        % constant matrix
        B = [-fx*dx - kx*x + m1*r*dphi1^2*cos(phi1) - m2*r*dphi2^2*cos(phi2)
            %..
            ];
        dy(1:5,1) = u(2:2:10);    % velocities
        dy(6:10,1) = A\B;         % accelerations
    end

end

[AJ]

Thanks.

But how do you include the Te1 and Te2 which is the electromagnitude torque from the two motors?

I really appreciate if you could see this code and help me with the of A and B:

% function main
% define constants
clc; clear all;

M = 300;
m1 = 4; m2 = 4; 
J = 35;
kx = 154000;
ky = 154000;
ksi = 29000;
fx = 270;
fy = 270;
fsi = 34;
fd1 = 0.0001;
fd2 = 0.0001;
Li = 0.5;
r = 0.05;
np = 3;
Rs = 25.4;
Rr = 25.4;
Ls = 1.7;
Lr = 1.7;
Lm = 1.15;
U = 220;

K = (3*np*(Lm^2)* (U^2)) / ((Rs^2)*Rr)
Ts = Ls/Rs;
Tr = Lr/Rr;
Sig = 1 - ((Lm^2) / (Ls*Lr))


a0 = (1 - ((ws^2)*Ts*Tr*Sig)^2) + ((ws^2)*((Tr + Ts)^2));
a1 = (2*(1 - ((ws^2)*Ts*Tr*Sig))*ws*Ts*Tr*Sig) - (2*(ws*((Tr + Ts)*Tr)));
a2 = Tr^2 + ((ws^2) * (Ts^2) * (Tr^2) *(Sig^2));
%  



Z0 = [0,0,0,0,0,0,0,0,0,0]; 
[t,y] = ode45(@(t,y) myode(t,u,M,m1,m2,L1,L2,r,J,j1,j2,ws,K,a0,a1,a2,kx,ky,kpsi,fx,fy,fpsi),t,Z0);



    function dy = myode(t,u,M,m1,m2,L1,L2,r,J,j1,j2,ws,K,a0,a1,a2,kx,ky,kpsi,fx,fy,fpsi)
        x = u(1);
        dx = u(2);
        y = u(3);
        dy = u(4);
        Psi = u(5);
        dPsi = u(6);
        phi1 = u(7);
        dphi1 = u(8);
        phi2 = u(9);
        dphi2 = u(10);
        
        Te1 = (K*(ws-dphi1)) / ((a2*(dphi1^2))+(a1*dphi1)+a0);
        Te2 = (K*(ws-dphi2)) / ((a2*(dphi2^2))+(a1*dphi2)+a0);


        A = [M 0 0   -m1*r*sin(phi1)    m2*r*sin(phi2)
             0 M 0    m1*r*cos(phi1)    m2*r*cos(phi2)
             0 0 J    m1*r*L1*cos(phi1) m2*r*L2*cos(phi2)...
             % ...
            ];

        
        B = [-fx*dx - kx*x + m1*r*dphi1^2*cos(phi1) - m2*r*dphi2^2*cos(phi2)
            -fy*dy - ky*y + m1*r*dphi1^2*sin(phi1) - m2*r*dphi2^2*sin(phi2)
            -fpsi*dpsi - kpsi*psi - m1*r*L1*dphi1^2*sin(phi1) + m2*r*L2*dphi2^2*sin(phi2)
            %..
            ];
        dy(1:5,1) = u(2:2:10);    % velocities
        dy(6:10,1) = A\B;         % accelerations
    end

The process that is given by the paper:

What I am trying to get out of the EOM:

[darova]

Red is matrix coefficients (A matrix)

Green is constant variables (B matrix)

function du = myode(t,u, ...    % maybe change the of result variable
    % ...
    dy = u(4);
    % ..
    du(1:5,1) = u(2:2:10);    % velocities
    du(6:10,1) = A\B;         % accelerations
end

Also your initial conditions can't be all zero. You won't get a results. Should be some initial speed

[AJ]

Thank you for the answer. I worked on the code and I'm not really sure if it works. The program is keep running.

Can you please help me to understand that how I can choose the initial values?

Also, I defined the electromagnetic torque from the two motors as below with substituting dPhi1 and dPhi2. Does that make sence? Should I use the time (t) somewher?

clc; clear all;

M = 300;
m1 = 4; m2 = 4; m = [m1,m2];
J = 35;
kx = 154000; ky = 154000; ks = 29000; 
k = [kx, ky, ks];
fx = 270;
fy = 270;
fs = 34;
fd1 = 0.0001;
fd2 = 0.0001;
L1 = 0.5;
L2 = 0.5;
r = 0.05;
j1 = m1*r^2;j2 = m2*r^2;
np = 3;
Rs = 25.4;
Rr = 25.4;
Ls = 1.7;
Lr = 1.7;
Lm = 1.15;
U = 220;
M = 300;


K = (3*np*(Lm^2)* (U^2)) / ((Rs^2)*Rr)
Ts = Ls/Rs;
Tr = Lr/Rr;
Sig = 1 - ((Lm^2) / (Ls*Lr))
ws = 960;

a0 = (1 - ((ws^2)*Ts*Tr*Sig)^2) + ((ws^2)*((Tr + Ts)^2));
a1 = (2*(1 - ((ws^2)*Ts*Tr*Sig))*ws*Ts*Tr*Sig) - (2*(ws*((Tr + Ts)*Tr)));
a2 = Tr^2 + ((ws^2) * (Ts^2) * (Tr^2) *(Sig^2));
%  
t_end = 5; dt = 0.1; t  = 0:dt:t_end;

Z1 = 0;
Z2 = 0.001;
Z0 = [Z1,Z1,Z1,Z1,Z1,Z2,Z2,Z2,Z2,Z2]; 
[t,u] = ode45(@(t,u) myode2(t,u,M,m,k,fx,fy,fs,L1,L2,fd1,fd2,r,K,ws,a0,a1,a2,J,j1,j2),t,Z0);

function du = myode2(t,u,M,m,k,fx,fy,fs,L1,L2,fd1,fd2,r,K,ws,a0,a1,a2,J,j1,j2)

m1 = m(1);
m2 = m(2);
kx = k(1);ky = k(2);ks = k(3);

Te1 = (K*(ws-u(8))) / (a2*(u(8)^2) + a1*u(8) + a0);
Te2 = (K*(ws-u(10))) / (a2*(u(10)^2) + a1*u(10) + a0);

A = [M 0 0 -m1*r*sin(u(7)) m2*r*sin(u(9));...
     0 M 0 m1*r*cos(u(7)) m2*r*cos(u(9));...
     0 0 J -m1*r*L1*cos(u(7)) m2*r*L2*cos(u(9));...
     -m1*r*sin(u(7)) m1*r*cos(u(7)) -m1*r*L1*cos(u(7)) j1 0;...
     -m2*r*sin(u(9)) m2*r*cos(u(9)) -m2*r*L2*cos(u(9)) 0 j1];
 
B = [-kx*u(1)-fx*u(2)+m1*r*cos(u(7))*(u(8)^2)-m2*r*cos(u(9))*(u(10)^2) ;...
     -ky*u(3)-fy*u(4)+m1*r*sin(u(7))*(u(8)^2)+m2*r*sin(u(9))*(u(10)^2) ;...
     -ks*u(5)-fs*u(6)+m1*r*L1*sin(u(7))*(u(8)^2)+m2*r*L2*sin(u(9))*(u(10)^2) ;...
     Te1-fd1*u(8) ;...
     Te2-fd2*u(10)];



du(1:5,:) = u(2:2:10);    % velocities
du(6:10,:) = A\B;         % accelerations

[darova]

Your code looks ok. I tried this:

t = [0 0.5];
Z0 = [0 0 0 0 0 1 0 1 0 1];        % all angular velocities are ones

I also changed the sign before A(5,3) and coefficient A(5,5)

-m1*r*sin(u(7)) m1*r*cos(u(7)) -m1*r*L1*cos(u(7)) j1 0;...
-m2*r*sin(u(9)) m2*r*cos(u(9))  m2*r*L2*cos(u(9)) 0 j2];

And got this:

[AJ]

Thanks a lot. I really appreciate.

Is there any way to guess that what range the initial conditions are?

[darova]

Those values are something like M or kx. You can't guess them

Imagine you have something like that:

You want to see what will hapen if you loose green ball.

What will be the initial conditions (Velocity and angle)? Depends on what you want