octave :: double pendulum

octave_mechanics_singlependulum

\[x_1=l_1\sin\theta_1\] \[y_1=-l_1\cos\theta_1\] \[x_2=l_1\sin\theta_1+l_2\sin\theta_2\] \[y_2=-l_1\cos\theta_1-l_2\cos\theta_2\] \[T=\frac{1}{2}m_1l_1^2\dot\theta_1^2 +\frac{1}{2}m_2(l_1^2\dot\theta_1^2+l_2^2\dot\theta_2^2 +2l_1l_2\dot\theta_1\dot\theta_2\cos(\theta_1-\theta_2))\] \[U=-m_1gl_1\cos\theta_1-m_2g(l_1\cos\theta_1+l_2\cos\theta_2)\] \[L=T-U\] \[\frac{d}{dt}\frac{dL}{d\dot\theta}-\frac{dL}{d\theta}=0\] \[ \left[ \begin{array}{cc} (m_1+m_2)l_1 & m_2l_2\cos(\theta_1-\theta_2)\\ m_2l_1\cos(\theta_2-\theta_1) & m_2l_2 \end{array} \right] \left[ \begin{array}{cc} \ddot\theta_1\\ \ddot\theta_2 \end{array} \right] = -\left[ \begin{array}{cc} (m_1+m_2)g \sin \theta_1 & m_2l_2\dot\theta_2^2\sin(\theta_1-\theta_2)\\ m_2l_1\dot\theta_1^2\sin(\theta_2-\theta_1) & m_2g\sin \theta_2 \end{array} \right] \left[ \begin{array}{cc} 1\\ 1 \end{array} \right] \] \[ \left[ \begin{array}{cc} \ddot\theta_1\\ \ddot\theta_2 \end{array} \right] = -\left[ \begin{array}{cc} (m_1+m_2)l_1 & m_2l_2\cos(\theta_1-\theta_2)\\ m_2l_1\cos(\theta_2-\theta_1) & m_2l_2 \end{array} \right]^{-1} \left[ \begin{array}{cc} (m_1+m_2)g \sin \theta_1 & m_2l_2\dot\theta_2^2\sin(\theta_1-\theta_2)\\ m_2l_1\dot\theta_1^2\sin(\theta_2-\theta_1) & m_2g\sin \theta_2 \end{array} \right] \left[ \begin{array}{cc} 1\\ 1 \end{array} \right] \] \[ \frac{d}{dt} \left[ \begin{array}{cc} \theta_1(t)\\ \theta_2(t)\\ \dot\theta_1(t)\\ \dot\theta_2(t)\\ \end{array} \right] = \left[ \begin{array}{cc} \dot\theta_1(t)\\ \dot\theta_2(t)\\ \ddot\theta_1(t)\\ \ddot\theta_2(t)\\ \end{array} \right] , \left[ \begin{array}{cc} \theta_1(0)\\ \theta_2(0)\\ \dot\theta_1(0)\\ \dot\theta_2(0) \end{array} \right] = \left[ \begin{array}{cc} \theta_{10}\\ \theta_{20}\\ 0\\ 0 \end{array} \right] \]

Function: octave_mechanics_singlependulum ()

function out = octave_mechanics_doublependulum(g,m1,m2,l1,l2,x_k)
dt =  0.01;
%m1 = 1;
%m2 = 1;
%l1 = 0.5;
%l2 = 0.5;
%g = 9.8;
%x_k = [60/180*pi ;
%          0      ;
%          0      ;
%          0      ];
x = [];
tim =[];

for t = 0 : dt : 10
    A1=[(m1+m2)*l1 m2*l2*cos(x_k(1)-x_k(2));
         m2*l1*cos(x_k(2)-x_k(1))     m2*l2];
    B1=[(m1+m2)*g*sin(x_k(1)) m2*l2*(x_k(4))^2*sin(x_k(1)-x_k(2));
         m2*l1*(x_k(3))^2*sin(x_k(2)-x_k(1))     m2*g*sin(x_k(2))];
    C1=[1;
        1];
    D1=-(inv(A1)*B1)*C1;
    k1 = [ x_k(3);
           x_k(4); 
           D1(1);
           D1(2)] * dt;
    x_tmp = x_k + k1/2;
    A2=[(m1+m2)*l1 m2*l2*cos(x_tmp(1)-x_tmp(2));
         m2*l1*cos(x_tmp(2)-x_tmp(1))     m2*l2];
    B2=[(m1+m2)*g*sin(x_tmp(1)) m2*l2*(x_tmp(4))^2*sin(x_tmp(1)-x_tmp(2));
         m2*l1*(x_tmp(3))^2*sin(x_tmp(2)-x_tmp(1))     m2*g*sin(x_tmp(2))];
    C2=[1;
        1];
    D2=-(inv(A2)*B2)*C2;
    k2 = [ x_tmp(3);
           x_tmp(4); 
           D2(1);
           D2(2)] * dt;
    x_tmp = x_k + k2/2;
    A3=[(m1+m2)*l1 m2*l2*cos(x_tmp(1)-x_tmp(2));
         m2*l1*cos(x_tmp(2)-x_tmp(1))     m2*l2];
    B3=[(m1+m2)*g*sin(x_tmp(1)) m2*l2*(x_tmp(4))^2*sin(x_tmp(1)-x_tmp(2));
         m2*l1*(x_tmp(3))^2*sin(x_tmp(2)-x_tmp(1))     m2*g*sin(x_tmp(2))];
    C3=[1;
        1];
    D3=-(inv(A3)*B3)*C3;
    k3 = [ x_tmp(3);
           x_tmp(4); 
           D3(1);
           D3(2)] * dt;
    x_tmp = x_k + k3;
    A4=[(m1+m2)*l1 m2*l2*cos(x_tmp(1)-x_tmp(2));
         m2*l1*cos(x_tmp(2)-x_tmp(1))     m2*l2];
    B4=[(m1+m2)*g*sin(x_tmp(1)) m2*l2*(x_tmp(4))^2*sin(x_tmp(1)-x_tmp(2));
         m2*l1*(x_tmp(3))^2*sin(x_tmp(2)-x_tmp(1))     m2*g*sin(x_tmp(2))];
    C4=[1;
        1];
    D4=-(inv(A4)*B4)*C4;
    k4 = [ x_tmp(3);
           x_tmp(4); 
           D4(1);
           D4(2)] * dt;
    x_k1 = x_k + (k1 + 2 * k2 + 2 * k3 + k4 ) /6 ;
    x = [x x_k];
    tim = [tim t];
    x_k = x_k1;
endfor

plot(l1*sin(x(1, :)),-l1*cos(x(1, :)),l1*sin(x(1, :))+l2*sin(x(2, :)),-l1*cos(x(1, :))-l2*cos(x(2, :)));
out=0;
endfunction

octave_mechanics_doublependulum.m

octave_mechanics_doublependulum