End effector not visualized

Good morning everyone,

I have a problem with the inverse kinematc applied to a robot that I assembled in Simulink. As you can see, the robot has seven links and six revolute joints. To apply the analytical inverse kinematics I imported the robot rigid body tree in Matlab. The problem is that the final results seem not to count the last link even if I select the second kinematcs group (which clearly includes the End effector as body7). Hence, when I apply the direct kinematics to verify the end effector pose, the output results place the sixth joint in the desidered position rather than the end effector. I have another question too. As you can see from the pictures I have attached in the rigid body tree of the simulink model of the robot, between the sixth revolute joint and the last body ('EE') I have placed a weld joint; I did it because, otherwise, in the program reads a body tree only with six bodies and shows only the first kinematcs group, ending with the body6.

In conclusion, how can I run the IK applied to the robot of mine including the actual pose of the end effector?

Thank you in advence for your time, hoping I was clear enough.

---------------------------------------------------------------------------------------------------------------------------------------------

%---------

% MATLAB SCRIPT |

%---------

clear;close all;clc

%---------

% DATA |

%---------

% Unknown

% Ang: phi_1, phi_2, phi_3, phi_4, phi_5, phi_6

%--------------

% P_EE POSE |

%--------------

% EE Dispalcement from the robot base

x_EE = 8;

y_EE = -5;

z_EE = -3;

d_EE = [x_EE y_EE z_EE];

% Angles

phi_EE_x = -6*pi/5;

phi_EE_y = -3*pi/4;

phi_EE_z = pi/6;

Eul_ang = [phi_EE_x phi_EE_y phi_EE_z];

% P_EE pose

EE_pose = trvec2tform(d_EE)*eul2tform(Eul_ang,"ZYX");

%---------

% ROBOT |

%---------

% Links lengths

l_1 = 2.5;

l_2 = 3;

l_3 = 5;

l_4 = 3.5;

l_5 = 1.5;

l_6 = 1.5;

l_EE = 1;

[robot, robot_info] = importrobot('Robot_ES5_LP');

%----------------------

% INVERSE KINEMATICS |

%----------------------

aik = analyticalInverseKinematics(robot);

opts = showdetails(aik);

aik.KinematicGroup = opts(2).KinematicGroup;

showdetails(robot);

generateIKFunction(aik, 'robotIK');

ikConfig = robotIK(EE_pose);

% Angles in all their possible configurations

phi_1 = ikConfig(:,1);

phi_2 = ikConfig(:,2);

phi_3 = ikConfig(:,3);

phi_4 = ikConfig(:,4);

phi_5 = ikConfig(:,5);

phi_6 = ikConfig(:,6);

%------------------------------------

% DIRECT KINEMATICS (AS VALIDATION) |

%------------------------------------

% Validation for all the possible configurations

for i = 1:length(phi_1)

% Pose joint1

d_01 = [0; 0; l_1];

R_01 = [cos(phi_1(i)) cos(pi/2+phi_1(i)) 0;...

cos(pi/2-phi_1(i)) cos(phi_1(i)) 0;...

0 0 1];

H_01_1 = [eye(3,3) d_01;...

zeros(1,3) 1];

H_01_2 = [R_01 zeros(3,1);...

zeros(1,3) 1];

H_01_3 = [cos(pi/2) 0 cos(pi/2-pi/2) 0;...

0 1 0 0;...

cos(pi/2+pi/2) 0 cos(pi/2) 0;...

0 0 0 1];

H_01 = H_01_1*H_01_2*H_01_3;

% Pose joint2

d_12 = [0; 0; l_2];

R_12 = [cos(phi_2(i)) 0 cos(pi/2-phi_2(i));...

0 1 0;...

cos(pi/2+phi_2(i)) 0 cos(phi_2(i))];

H_12 = [R_12 d_12;...

zeros(1,3) 1];

% Pose joint3

d_23 = [0; 0; l_3];

R_23 = [cos(phi_3(i)) 0 cos(pi/2-phi_3(i));...

0 1 0;...

cos(pi/2+phi_3(i)) 0 cos(phi_3(i))];

H_23 = [R_23 d_23;...

zeros(1,3) 1];

% Pose joint4

d_34 = [0; 0; l_4];

R_34 = [cos(phi_4(i)) cos(pi/2+phi_4(i)) 0;...

cos(pi/2-phi_4(i)) cos(phi_4(i)) 0;...

0 0 1];

H_34 = [R_34 d_34;...

zeros(1,3) 1];

% Pose joint5

d_45 = [0; 0; l_5];

R_45 = [cos(phi_5(i)) 0 cos(pi/2-phi_5(i));...

0 1 0;...

cos(pi/2+phi_5(i)) 0 cos(phi_5(i))];

H_45 = [R_45 d_45;...

zeros(1,3) 1];

% Pose joint6

d_56 = [0; 0; l_6];

R_56 = [cos(phi_6(i)) cos(pi/2+phi_6(i)) 0;...

cos(pi/2-phi_6(i)) cos(phi_6(i)) 0;...

0 0 1];

H_56 = [R_56 d_56;...

zeros(1,3) 1];

% Pose EE

s_EE = [0; 0; l_EE];

H_EE = [eye(3,3) s_EE;...

zeros(1,3) 1];

%------------------------------------

% BASE, JOINTS AND E-E COORDINATES |

%------------------------------------

% Base cordinates

x_0 = 0;

y_0 = 0;

z_0 = 0;

P_0 = [x_0; y_0; z_0];

% P_1 cordinates

P_1 = H_01*[P_0; 1];

% P_2 cordinates

P_2 = H_01*H_12*[P_0; 1];

% P_3 cordinates

P_3 = H_01*H_12*H_23*[P_0; 1];

% P_4 cordinates

P_4 = H_01*H_12*H_23*H_34*[P_0; 1];

% P_5 cordinates

P_5 = H_01*H_12*H_23*H_34*H_45*[P_0; 1];

% P_6 cordinates

P_6 = H_01*H_12*H_23*H_34*H_45*H_56*[P_0; 1];

% P_EE cordinates

P_EE = H_01*H_12*H_23*H_34*H_45*H_56*H_EE*[P_0; 1];

%--------------------

% REFERENCE FRAMES |

%--------------------

% Reference frame 0

u_0 = [1; 0; 0];

v_0 = [0; 1; 0];

w_0 = [0; 0; 1];

% Reference frame 1

u_1 = H_01*[u_0; 1];

v_1 = H_01*[v_0; 1];

w_1 = H_01*[w_0; 1];

% Reference frame 2

u_2 = H_01*H_12*[u_0; 1];

v_2 = H_01*H_12*[v_0; 1];

w_2 = H_01*H_12*[w_0; 1];

% Reference frame 3

u_3 = H_01*H_12*H_23*[u_0; 1];

v_3 = H_01*H_12*H_23*[v_0; 1];

w_3 = H_01*H_12*H_23*[w_0; 1];

% Reference frame 4

u_4 = H_01*H_12*H_23*H_34*[u_0; 1];

v_4 = H_01*H_12*H_23*H_34*[v_0; 1];

w_4 = H_01*H_12*H_23*H_34*[w_0; 1];

% Reference frame 5

u_5 = H_01*H_12*H_23*H_34*H_45*[u_0; 1];

v_5 = H_01*H_12*H_23*H_34*H_45*[v_0; 1];

w_5 = H_01*H_12*H_23*H_34*H_45*[w_0; 1];

% Reference frame 6

u_6 = H_01*H_12*H_23*H_34*H_45*H_56*[u_0; 1];

v_6 = H_01*H_12*H_23*H_34*H_45*H_56*[v_0; 1];

w_6 = H_01*H_12*H_23*H_34*H_45*H_56*[w_0; 1];

% Reference frame 6

u_EE = H_01*H_12*H_23*H_34*H_45*H_56*H_EE*[u_0; 1];

v_EE = H_01*H_12*H_23*H_34*H_45*H_56*H_EE*[v_0; 1];

w_EE = H_01*H_12*H_23*H_34*H_45*H_56*H_EE*[w_0; 1];

%----------------------------

% GRAPHICAL REPRESENTATION |

%----------------------------

figure(i);

% Link 1

plot3([P_0(1) P_1(1)],[P_0(2) P_1(2)],[P_0(3) P_1(3)],'ko-');hold on;

% Link 2

plot3([P_1(1) P_2(1)],[P_1(2) P_2(2)],[P_1(3) P_2(3)],'ko-');hold on;

% Link 3

plot3([P_2(1) P_3(1)],[P_2(2) P_3(2)],[P_2(3) P_3(3)],'ko-');hold on;

% Link 4

plot3([P_3(1) P_4(1)],[P_3(2) P_4(2)],[P_3(3) P_4(3)],'ko-');hold on;

% Link 5

plot3([P_4(1) P_5(1)],[P_4(2) P_5(2)],[P_4(3) P_5(3)],'ko-');hold on;

% Link 6

plot3([P_5(1) P_6(1)],[P_5(2) P_6(2)],[P_5(3) P_6(3)],'ko-');hold on;

% EE

plot3([P_6(1) P_EE(1)],[P_6(2) P_EE(2)],[P_6(3) P_EE(3)],'ko-');hold on;

% Reference frame base

plot3([u_0(1) P_0(1)],[u_0(2) P_0(2)],[u_0(3) P_0(3)],'r-');hold on;

plot3([v_0(1) P_0(1)],[v_0(2) P_0(2)],[v_0(3) P_0(3)],'g-');hold on;

plot3([w_0(1) P_0(1)],[w_0(2) P_0(2)],[w_0(3) P_0(3)],'b-');hold on;

% Reference frame 1

plot3([u_1(1) P_1(1)],[u_1(2) P_1(2)],[u_1(3) P_1(3)],'r-');hold on;

plot3([v_1(1) P_1(1)],[v_1(2) P_1(2)],[v_1(3) P_1(3)],'g-');hold on;

plot3([w_1(1) P_1(1)],[w_1(2) P_1(2)],[w_1(3) P_1(3)],'b-');hold on;

% Reference frame 2

plot3([u_2(1) P_2(1)],[u_2(2) P_2(2)],[u_2(3) P_2(3)],'r-');hold on;

plot3([v_2(1) P_2(1)],[v_2(2) P_2(2)],[v_2(3) P_2(3)],'g-');hold on;

plot3([w_2(1) P_2(1)],[w_2(2) P_2(2)],[w_2(3) P_2(3)],'b-');hold on;

% Reference frame 3

plot3([u_3(1) P_3(1)],[u_3(2) P_3(2)],[u_3(3) P_3(3)],'r-');hold on;

plot3([v_3(1) P_3(1)],[v_3(2) P_3(2)],[v_3(3) P_3(3)],'g-');hold on;

plot3([w_3(1) P_3(1)],[w_3(2) P_3(2)],[w_3(3) P_3(3)],'b-');hold on;

% Reference frame 4

plot3([u_4(1) P_4(1)],[u_4(2) P_4(2)],[u_4(3) P_4(3)],'r-');hold on;

plot3([v_4(1) P_4(1)],[v_4(2) P_4(2)],[v_4(3) P_4(3)],'g-');hold on;

plot3([w_4(1) P_4(1)],[w_4(2) P_4(2)],[w_4(3) P_4(3)],'b-');hold on;

% Reference frame 5

plot3([u_5(1) P_5(1)],[u_5(2) P_5(2)],[u_5(3) P_5(3)],'r-');hold on;

plot3([v_5(1) P_5(1)],[v_5(2) P_5(2)],[v_5(3) P_5(3)],'g-');hold on;

plot3([w_5(1) P_5(1)],[w_5(2) P_5(2)],[w_5(3) P_5(3)],'b-');hold on;

% Reference frame 6

plot3([u_6(1) P_6(1)],[u_6(2) P_6(2)],[u_6(3) P_6(3)],'r-');hold on;

plot3([v_6(1) P_6(1)],[v_6(2) P_6(2)],[v_6(3) P_6(3)],'g-');hold on;

plot3([w_6(1) P_6(1)],[w_6(2) P_6(2)],[w_6(3) P_6(3)],'b-');hold on;

% Reference frame EE

plot3([u_EE(1) P_EE(1)],[u_EE(2) P_EE(2)],[u_EE(3) P_EE(3)],'r-');hold on;

plot3([v_EE(1) P_EE(1)],[v_EE(2) P_EE(2)],[v_EE(3) P_EE(3)],'g-');hold on;

plot3([w_EE(1) P_EE(1)],[w_EE(2) P_EE(2)],[w_EE(3) P_EE(3)],'b-');hold on;...

axis equal;title(['Configuration ' num2str(i)],'Interpreter','latex');...

xlabel('$x_0$','Interpreter','latex');ylabel('$y_0$','Interpreter','latex');...

zlabel('$z_0$','Interpreter','latex');

end

phi_IK = (180/pi)*ikConfig