octave_mechanics_triplependulum
function out = octave_mechanics_triplependulum()
dt = 0.01;
m1 = 1;
m2 = 1;
m3 = 1;
l1 = 0.5;
l2 = 0.5;
l3 = 0.5;
g = 9.8;
x_k = [60/180*pi ;
0 ;
0 ;
0 ;
0 ;
0 ];
x = [];
tim =[];
for t = 0 : dt : 10
x_k1 = x_k;
x_tmp = x_k;
for j =1 : 1 : 4
A=[(m1+m2+m3)*l1 (m2+m3)*l2*cos(x_tmp(1)-x_tmp(2)) m3*l3*cos(x_tmp(1)-x_tmp(3));
(m2+m3)*l1*cos(x_tmp(2)-x_tmp(1)) (m2+m3)*l2 m3*l3*cos(x_tmp(2)-x_tmp(3));
m3*l1*cos(x_tmp(3)-x_tmp(1)) m3*l2*cos(x_tmp(3)-x_tmp(2)) m3*l3 ];
B=[(m1+m2+m3)*g*sin(x_tmp(1)) (m2+m3)*l2*(x_tmp(5))^2*sin(x_tmp(1)-x_tmp(2)) m3*l3*(x_tmp(6))^2*sin(x_tmp(1)-x_tmp(3));
(m2+m3)*l1*(x_tmp(4))^2*sin(x_tmp(2)-x_tmp(1)) (m2+m3)*g*sin(x_tmp(2)) m3*l3*(x_tmp(6))^2*sin(x_tmp(2)-x_tmp(3));
m3*l1*(x_tmp(4))^2*sin(x_tmp(3)-x_tmp(1)) m3*l2*(x_tmp(5))^2*sin(x_tmp(3)-x_tmp(2)) m3*g*sin(x_tmp(3))];
C=[1;
1;
1];
D=-(inv(A)*B)*C;
k_j = [ x_tmp(4);
x_tmp(5);
x_tmp(6);
D(1);
D(2);
D(3)] * dt;
if j == 1
x_tmp = x_k + k_j /2;
endif
if j == 2
x_tmp = x_k + k_j /2;
endif
if j == 3
x_tmp = x_k + k_j ;
endif
if j == 4
x_tmp = x_k + k_j /2;
endif
if j == 1
x_k1 = x_k1 + k_j /6;
endif
if j == 2
x_k1 = x_k1 + k_j /3;
endif
if j == 3
x_k1 = x_k1 + k_j /3;
endif
if j == 4
x_k1 = x_k1 + k_j /6;
endif
endfor
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,:)),l1*sin(x(1,:))+l2*sin(x(2,:))+l3*sin(x(3,:)),-l1*cos(x(1,:))-l2*cos(x(2,:))-l3*cos(x(3,:)));
out=0;
endfunction
