Code covered by the BSD License  

Highlights from
ANFIS for 2 dof robot

ANFIS for 2 dof robot

by

 

07 May 2012 (Updated )

anfis function is used to create a neural network to solve the inverse kinematics problem

trialdata.m
global s12 data1 data2

%s12 is a counter used in the function coordinates to select the rows of
%the matrix
s12=0;   

%errorx is used to store the error. it is not necessary from my point of
%view to initialize this variable and this step can be skipped
errorx=[];

%creating the training data. Here array of the two joint angles are
%created. One can increase the resolution and hence accuracy of the trained
%FIS by having more input joint angles as training data. less training data
%faster the trainning process but with less accuracy in the solution
qt5=0:45*pi/180:pi/2 ;    % change from 30 to 60 for faster training
qt6=0:90*pi/180:pi;      % range changed from (-180 to 180) to (0 to 90)
[QT5 QT6]=meshgrid(qt5,qt6);

%here only one variable is selected as after meshgrid both the variables
%will have the same size. the answer is then used to send data one by one
%to the DK model
[ro col]=size(QT5);      
for r=1:1:ro
    for c=1:1:col
DK47(QT5(r,c),QT6(r,c))    %passing the data to the function containing the DK model
    end
end

%%
% The first parameter to |anfis| is the training data, the second parameter
% is the number of membership functions used to characterize each input and
% output, the third parameter is the number of training <#23 epochs> and
% the last parameter is the options to display progress during training.
% The values for number of epochs and the number of membership functions
% have been arrived at after a fair amount of experimentation with
% different values.

fprintf('-->%s\n','Start training first ANFIS network. It may take one minute depending on your computer system.')
anfis1 = anfis(data1, 7, 50, [0,0,0,0]); % train first ANFIS network
fprintf('-->%s\n','Start training second ANFIS network. It may take one minute depending on your computer system.')
anfis2 = anfis(data2, 6, 50, [0,0,0,0]); % train second ANFIS network

%NOTE: my experience of the member function is more the member function
%more accurate the result but more training time required. same is with the
%epochs
%% Creating check data and checking the error
% the joint angle data created earlier is taken and put in the DK model and
% from where we get the final positions for the respective joint angles.
% then we put in the joint angles in the 'EVALFIS' and obtain the answer.
% At the end error is plotted in along all three axis
for r=1:1:ro
    for c=1:1:col
q5=QT5(r,c);
q6=QT6(r,c);
d5=550;
q7=pi;
T4_5=[cos(q5) 0 sin(q5) 0
    sin(q5) 0 -cos(q5) 0
    0 1 0 d5
    0 0 0 1];
T5_6=[cos(q6) 0 -sin(q6) 0
    sin(q6) 0 cos(q6) 0
    0 -1 0 0
    0 0 0 1];
T6_7=[cos(q7) -sin(q7) 0 0
    sin(q7) cos(q7) 0 0
    0 0 1 170
    0 0 0 1];
T4_7=T4_5*T5_6*T6_7;
XYZ=T4_7(1:3,4)';
THETA1P = evalfis(XYZ, anfis1); % theta1 predicted by anfis1
THETA2P = evalfis(XYZ, anfis2); % theta2 predicted by anfis2
error1=q5-THETA1P;
error2=q6-THETA2P;
%% error calculation in the final position of the 2 link robot
q5=THETA1P;
q6=THETA2P;
T4_5=[cos(q5) 0 sin(q5) 0
    sin(q5) 0 -cos(q5) 0
    0 1 0 d5
    0 0 0 1];
T5_6=[cos(q6) 0 -sin(q6) 0
    sin(q6) 0 cos(q6) 0
    0 -1 0 0
    0 0 0 1];
T6_7=[cos(q7) -sin(q7) 0 0
    sin(q7) cos(q7) 0 0
    0 0 1 170
    0 0 0 1];
T4_7=T4_5*T5_6*T6_7;
XYZN=T4_7(1:3,4)';
errorx(r,c)=XYZ(1,1)-XYZN(1,1);  % calculate  the error is the final position
errory(r,c)=XYZ(1,2)-XYZN(1,2);
errorz(r,c)=XYZ(1,3)-XYZN(1,3);
    end
end
subplot(1,3,1)
plot3(QT5,QT6,errorx)
xlabel('QT5')
ylabel('QT6')
zlabel('errorx')
title('Plot of error along x direction')
subplot(1,3,2)
plot3(QT5,QT6,errory)
xlabel('QT5')
ylabel('QT6')
zlabel('errory')
title('Plot of error along y direction')
subplot(1,3,3)
plot3(QT5,QT6,errorz)
xlabel('QT5')
ylabel('QT6')
zlabel('errorz')
title('Plot of error along z direction')

Contact us