twolink_sim.m

dynamic analysis of flexible 2 bar serial robot by De-noc

% ndx : index
% rho : mass per unit length
% a, b, alp, theta : DH parameters
clear all; 
clc;
global a b rho alp n m tau c1 c2 c3 c4 c5 c6 c7 c8 Z3 O1 O2 O3 Mvv1 Mvw1 Mww1...
N Nd Ne Ky xab zaa Ihi mai

%common constants

xab=[1;0;0];
zaa=[0;0;1];

Z3=zeros(3);
O1=eye(3);
O2=[ 0 -1 0
     1  0 0
     0  0 0];
O3=[-1  0 0
     0 -1 0
     0  0 0];
E=2e11;
G=80e9;

n=2;
m=2;
a=[1 1];
b=[0 0];
theta=[-pi/2 5*pi/180];
% theta=[0 0];
alp=[0 0];
tau=[0 0];
Ihi=[0 0];
mai=[0 0];

for i=1:n
%   rho(i)=7800*a(i)*b(i);
    rho(i)=5;
    c1(i)=-rho(i)*b(i)*(a(i)+b(i)/2);
    c2(i)=-rho(i)*a(i)*a(i)/2;
    c3(i)=-rho(i)*b(i)*b(i)*(b(i)/3+a(i));
    c4(i)=-rho(i)*a(i)*a(i)*a(i)/3;
    c5(i)=-rho(i)*a(i)*a(i)*b(i);
    c6(i)=-rho(i)*b(i)*b(i);
    c7(i)=-rho(i)*b(i)*a(i)*b(i);
    c8(i)=-rho(i)*b(i);   
    
    Mvv1(:,:,i)=rho(i)*(a(i)+b(i))*O1 + mai(i);
    Mvw1(:,:,i)=c1(i)*O2;
    Mww1(:,:,i)=c3(i)*O3;
    
    le=a(i)/m;
    %shape functions
    ncE = [le/2 le^2/12 le/2 -le^2/12];
    ndE = [3/20*le^2 1/30*le^3 7/20*le^2 -1/20*le^3];
    neE = [    13/35*le  11/210*le^2      9/70*le -13/420*le^2
            11/210*le^2   1/105*le^3  13/420*le^2  -1/140*le^3 
                9/70*le  13/420*le^2     13/35*le -11/210*le^2
           -13/420*le^2  -1/140*le^3 -11/210*le^2   1/105*le^3];
            
    k1E  = [  12   6*le   -12   6*le
            6*le 4*le^2 -6*le 2*le^2
             -12  -6*le    12  -6*le
            6*le 2*le^2 -6*le 4*le^2];
    
%     ncE=zeros(1,4);
%     ndE=zeros(1,4);
%     neE=zeros(4,4);
%     nfE=zeros(3,3);
%     k1E=zeros(4,4);
%     k3E=zeros(3,3);
    

    % Assembly of shape functions
    ndx=3;
    nc(1,ndx:ndx+3)        =ncE; nd(1,ndx:ndx+3)        =ndE;
    ne(ndx:ndx+3,ndx:ndx+3)=neE; k1(ndx:ndx+3,ndx:ndx+3)=k1E;
    ndx=1;
    nc(1,ndx:ndx+3)         = nc(1,ndx:ndx+3)         + ncE;
    nd(1,ndx:ndx+3)         = nd(1,ndx:ndx+3)         + ndE;
    ne(ndx:ndx+3,ndx:ndx+3) = ne(ndx:ndx+3,ndx:ndx+3) + neE;
    k1(ndx:ndx+3,ndx:ndx+3) = k1(ndx:ndx+3,ndx:ndx+3) + k1E;
    %Boundary condition
    N(i,:)    = nc(1,3:2*(m+1));
    Nd(i,:)   = nd(1,3:2*(m+1));
    Ne(:,:,i) = ne(3:2*(m+1),3:2*(m+1));
    
    % Stiffness matrix calculation
%     Iz=width(i)*thick(i)*thick(i)*thick(i)/12; % Area moment of intertia Z
%     Ky(:,:,i) = (E*Iz/le^3)*k1(3:2*(m+1),3:2*(m+1));
    Ky(:,:,i) = (50000/le^3)*k1(3:2*(m+1),3:2*(m+1));
  
end

in_con=[theta(1);0;0;0;0;theta(2);0;0;0;0;0;0;0;0;0;0;0;0;0;0];
% Initial condition state space vector composition
% in_con=eye(2*n*(2*m+1),1);
% for i=1:n
%     in_con((i-1)*(2*m+1)+1,1)=theta(i);
%     in_con((n+i-1)*(2*m+1)+1,1)=1;
% end




% in_con(2:5)=inc(1:4);
% in_con(7:10)=inc(6:9);
% Solving of DE
% twolink_fwd_dyn([0 0.05],in_con)
[T,Y]=ode45(@twolink_fwd_dyn,[0 2],in_con);
plot(T,Y(:,1));
grid on;
xlabel('Time(sec)');
ylabel('Joint ang (rad)');
figure;
plot(T,Y(:,6));
grid on;
xlabel('Time(sec)');
ylabel('Joint ang (rad)');
figure;
plot(T,Y(:,11));
grid on;
xlabel('Time(sec)');
ylabel('Joint rate (rad/s)');
figure;
plot(T,Y(:,16));
grid on;
xlabel('Time(sec)');
ylabel('Joint rate (rad/s)');
figure;
plot(T,Y(:,4));
grid on;
xlabel('Time(sec)');
ylabel('Deflection (m)');
figure;
plot(T,Y(:,8));
grid on;
xlabel('Time(sec)');
ylabel('Deflection (m)');
%check 1star