bar3.asv

Inverse dynamic analysis of 3 bar serial robot using new efficient method De-NOC.

clc;clear;
m1=2;m2=1.1;m3=1.6;m4=3;pi=3.14;
a1=0.7;a2=0.8;a3=1.1;a4=0.6;a5=1.2;i=1;
for t=0:0.1:10;
  th1=pi*(t-(10/(2*pi))*sin(2*pi*t/10))/10;
  dth1=pi*(1-cos(2*pi*t/10))/10;
  ddth1=(pi)*(2*pi/10)*sin(2*pi*t/10)/10;
  th2=pi*(t-(10/(2*pi))*sin(2*pi*t/10))/10;
  dth2=pi*(1-cos(2*pi*t/10))/10;
  ddth2=(pi)*(2*pi/10)*sin(2*pi*t/10)/10;
  th3=pi*(t-(10/(2*pi))*sin(2*pi*t/10))/10;
  dth3=pi*(1-cos(2*pi*t/10))/10;
  ddth3=(pi)*(2*pi/10)*sin(2*pi*t/10)/10;
% Coordinate frames
i1=[1;0;0];
j1=[0;1;0];
e1=[0;0;1];e2=[0;0;1];e3=[0;0;1];e5=[0;0;1];

Izz1=1/3*m1*a1^2;Izz2=1/3*m2*a2^2;Izz3=1/3*m3*a3^2;Izz4=1/3*m4*a4^2;

% Rotation matrices

Q1 =[cos(th1) -sin(th1)  0
     sin(th1)  cos(th1)  0
        0          0     1];
Q2 =[cos(th2) -sin(th2)  0
     sin(th2)  cos(th2)  0
        0          0     1];
Q3 =[cos(th3) -sin(th3)  0
     sin(th3)  cos(th3)  0
        0          0     1];
    
    a12=[a1;0;0];a23=[a2;0;0];a34=[a3;0;0];a45=[a4;0;0];             %aij (i-link , j-frame)
a11=Q1*a12;                            
a21=Q1*Q2*a23;
a31=Q1*Q2*Q3*a34;


C31=-(a21+a31);

% SUBSYSTEM I

% Mass matrices
I1=[Izz1*eye(3,3)      zeros(3,3)             
    zeros(3,3)         m1*eye(3,3)];       
I2=[Izz2*eye(3,3)      zeros(3,3)            
    zeros(3,3)         m2*eye(3,3)];
I3=[Izz3*eye(3,3)      zeros(3,3)            
    zeros(3,3)         m3*eye(3,3)];
I4=[Izz4*eye(3,3)      zeros(3,3)            
    zeros(3,3)         m4*eye(3,3)];
Ms1 =[   I1        zeros(6,6)      zeros(6,6)
      zeros(6,6)     I2            zeros(6,6)
      zeros(6,6)     zeros(6,6)       I2     ];
  
% pi, Joint-rate propogation matrices (motion of ith joint)
p1=[e1
        cross(e1,a11)];
p2=[e2
        cross(e2,a21)];
p3=[e3
        cross(e3,a31)];
    
    % Bij, Twist propogation matrices (propogates the twist of #j to #i)
scewC21=-1*[0      -a21(3)  a21(2)
            a21(3)    0    -a21(1)
           -a21(2)  a21(1)    0  ];
scewC32=-1*[0      -a31(3)  a31(2)
            a31(3)    0    -a31(1)
           -a31(2)  a31(1)    0  ];
scewC31=[0      -C31(3)  C31(2)
         C31(3)    0    -C31(1)
        -C31(2)  C31(1)    0  ];
  
B21=[eye(3,3)      zeros(3,3)
     scewC21       eye(3,3) ];
B32=[eye(3,3)      zeros(3,3)
     scewC32       eye(3,3) ];
B31=[eye(3,3)      zeros(3,3)
     scewC31       eye(3,3) ];
 
% De-NOC matrices
Nl =[eye(6,6)         zeros(6,6)        zeros(6,6)
     B21              eye(6,6)          zeros(6,6)
     B31              B32               eye(6,6)  ];
Nd =[p1               zeros(6,1)        zeros(6,1)
     zeros(6,1)          p2             zeros(6,1)
     zeros(6,1)       zeros(6,1)           p3     ];
N=Nl*Nd;

% Genralised Inertia matrix (GIM)
% M telda
Mt =transpose(Nl)*Ms1*(Nl);
Is1=transpose(Nd)*Mt*Nd

% Matrix of convective inertia terms (MCI)
dC21=-(dth1+dth2)*cross(e2,a21);
dC32=-(dth1+dth2+dth3)*cross(e3,a31);
dC31=-((dth1+dth2)*cross(e2,a21)+(dth1+dth2+dth3)*cross(e3,a31));

scewdC21=[0       -dC21(3)  dC21(2)
         dC21(3)    0      -dC21(1)
        -dC21(2)  dC21(1)     0   ];
scewdC32=[0       -dC32(3)  dC32(2)
         dC32(3)    0      -dC32(1)
        -dC32(2)  dC32(1)     0   ];
scewdC31=[0       -dC31(3)  dC31(2)
         dC31(3)    0      -dC31(1)
        -dC31(2)  dC31(1)     0   ];
    
dB21 =[zeros(3,3)        zeros(3,3)
       scewdC21          zeros(3,3)];
dB31 =[zeros(3,3)        zeros(3,3)
       scewdC31          zeros(3,3)];
dB32 =[zeros(3,3)        zeros(3,3)
       scewdC32          zeros(3,3)];

dNl=[zeros(6,6)         zeros(6,6)       zeros(6,6)
      dB21              zeros(6,6)       zeros(6,6)
      dB31                dB32           zeros(6,6)];
 
Mlt =transpose(Nd)*(transpose(Nl)*Ms1*(dNl))*Nd
scewde1=[0       -e1(3)  e1(2)
        e1(3)    0      -e1(1)
        -e1(2)  e1(1)     0   ];
Ws1 =[dth1*scewde1       zeros(3,3)       zeros(3,3)      zeros(3,3)      zeros(3,3)      zeros(3,3)  % genralised matrix of angular velocities
         zeros(3,3)      zeros(3,3)       zeros(3,3)      zeros(3,3)      zeros(3,3)      zeros(3,3)
         zeros(3,3)      zeros(3,3)      dth2*scewde1     zeros(3,3)      zeros(3,3)      zeros(3,3)
         zeros(3,3)      zeros(3,3)       zeros(3,3)      zeros(3,3)      zeros(3,3)      zeros(3,3)
         zeros(3,3)      zeros(3,3)       zeros(3,3)      zeros(3,3)      dth3*scewde1    zeros(3,3) 
         zeros(3,3)      zeros(3,3)       zeros(3,3)      zeros(3,3)      zeros(3,3)      zeros(3,3)];
Met=transpose(Nd)*transpose(Nl)*Ws1*Ms1*(Nl)*Nd

dp1= [zeros(3,1)                           
      dth1*cross(e1,cross(e1,a11))]; 
dp2= [zeros(3,1)                           
      (dth1+dth2)*cross(e2,cross(e2,a21))];
dp3= [zeros(3,1)                           
      (dth1+dth2+dth3)*cross(e3,cross(e3,a31))];
  
dNd=[dp1              zeros(6,1)      zeros(6,1)                                            
     zeros(6,1)         dp2           zeros(6,1)
     zeros(6,1)       zeros(6,1)         dp3    ];
Mwt=transpose(Nd)*Mt*dNd

Cs1  =Mlt+Met+Mwt;
dths1 =[dth1
        dth2
        dth3];
h=Cs1*dths1
% External wrench
g=9.81*[0;-1;0];f3=0;
f3e=f3*[0;-1;0];
% Wes1 =[tue1
%        m1*g
%        zeros(3,1)
%        m2*g
%        zeros(3,1)
%        Q1*Q2*Q3*f3e+m3*g];

% Constrained wrench
%lmd =[lmx;lmy;0];
scewa31=[0       -a31(3)  a31(2)
         a31(3)    0     -a31(1)
        -a31(2)   a31(1)    0   ];
wlms1 =[zeros(3,3)
        zeros(3,3)
        zeros(3,3)
        zeros(3,3)
        scewa31
        eye(3,3)  ];
wlms1(9:18,1:3)
tlms1=transpose(N)*wlms1;
% tolms1=[tlms1(1,1) tlms1(1,2);tlms1(2,1) tlms1(2,2)];
dths1 =[dth1
        dth2
        dth3];   
We=[m1*cross(g,a11/2);m1*g;m2*cross(g,a21/2);m2*g;m3*cross(g,a31/2);m3*g]
C1=(Is1*[ddth1;ddth2;ddth3]+Cs1*dths1)-We;
to1(i)=C1(1,1);to2(i)=C1(2,1);to3(i)=C1(3,1);
% torque1=[C1(3,1);C1(9,1);C1(15,1)]
i=i+1;
% pinv(transpose(Nd)*transpose(Nl))*
end

figure(2)
t= 0:0.1:10;
plot(t,torque1(1,1),'r',t,torque1(2,1),'b',t,torque1(3,1),'g','Linewidth',2);
title('Joint cons force')
xlabel('Time (sec)');
ylabel('force (N) ');
grid on
figure(3)
t= 0:0.1:10;
plot(t,th1,'r',t,th2,'b',t,th3,'g','Linewidth',2);
title('Joint cons force')
xlabel('Time (sec)');
ylabel('force (N) ');
grid on