function dx = odefun(t, x, kk1, kk2)
kk1 ; kk2 ;
l1 = 10*10^(-6) ;
l2 = 10*10^(-6) ;
m0 = 10^(-10) ;
m1 = l1*10^(-6) ;
m2 = l2*10^(-6) ;
k1 = 10^(-kk1)/l1 ;
k2 = 10^(-(kk1+kk2-2))/l2 ;
Crot = 10^(0) ;
Ctrans = 10^(-7) ;
thetaf1 = 20*pi/180 ;
thetaf2 = 40*pi/180 ;
L = 10^(-4) ;
L1 = l1/L ;
L2 = l2/L ;
M = m0+m1+m2 ;
M12 = (m1+m2)/M ;
M2 = m2/M ;
C1 = (Crot*L)/(k1*M)^0.5 ;
C2 = ( Ctrans+(l1+l2)*Crot )*(L/(k1*M)^0.5) ;
dx = zeros(size(x)) ;
dx(1) = x(4) ;
dx(2) = x(5) ;
dx(3) = x(6) ;
m11 = 1 ;
m12 = -M12*L1*sin(x(2)) ;
m13 = -M2*L2*sin(x(3)) ;
m22 = M12*(L1)^2 ;
m23 = M2*L1*L2*cos(x(3) - x(2)) ;
m33 = M2*(L2)^2 ;
kcc = 1.5 ;
if x(2) < 0
kc1 = kcc ;
else
kc1 = 1 ;
end
if x(3) - x(2) < 0
kc2 = kcc ;
else
kc2 = 1 ;
end
K1 = ( thetaf1 - x(2) ) ;
K2 = ( kc2*k2/(kc1*k1) )*( thetaf2 - x(3) ) ;
Rx = -( (C2+C1*(L1+L2))*x(4) - C1*( (L1+L2)*L1*x(5)*sin(x(2)) + (L2^2)*x(6)*sin(x(3)) ) ) ;
R1 = -C1*( -(L1+L2)*L1*x(4)*sin(x(2)) + (L1+L2)*(L1^2)*x(5) + L1*(L2^2)*x(6)*cos(x(3) - x(2)) ) ;
R2 = -C1*( -(L2^2)*x(4)*sin(x(3)) + (L2^3)*x(6) + L1*(L2^2)*x(5)*cos(x(3) - x(2)) ) ;
M = [ m22*m33-m23^2 -(m12*m33-m13*m23) m12*m23-m13*m22 ;
-(m12*m33-m23*m13) m11*m33-m13^2 -(m11*m23-m13*m12) ;
m12*m23-m22*m13 -(m11*m23-m12*m13) m11*m22-m12^2 ] ./ ( m11*m22*m33 + 2*m12*m13*m23 - m22*m13^2 - m11*m23^2 - m33*m12^2 ) ;
A = [ M12*L1*(x(5)^2)*cos(x(2)) + M2*L2*(x(6)^2)*cos(x(3)) + Rx ;
M2*L1*L2*(x(6)^2)*sin(x(3) - x(2)) + K1 + R1 ;
-M2*L1*L2*(x(5)^2)*sin(x(3) - x(2)) + K2 + R2 ] ;
dx(4:6) = M*A ;
end
clear all;
initCond = [ 0 0 0 0 0 0 ];
opts = odeset( 'RelTol', 1e-6, 'AbsTol', 1e-4 ) ;
for kk1 = 10:1:20
for kk2 = 1:1:3
solsA = ode15s( @odefun, [0 10^7], initCond, opts, kk1, kk2 ) ;
A = solsA.y(1,end) ;
end
end