Buongiorno a tutti,
Ho un problema con la cinematica inversa applicata ad un robot che ho assemblato in Simulink. Come potete vedere dalle immagini in allegato, il robot ha sette links e sei giunti di rivoluzione. Per applicare la cinematica analitica inversa ho importato l'albero del robot in Matlab. Il problema è che i risultati finali sembrano non contare l'ultimo link anche se seleziono il secondo gruppo cinematografico (che include chiaramente il end-effector come “body7”). Quindi, quando applico la cinematica diretta per verificare la posa dell'end-effector, i risultati posizionano il sesto giunto nella posizione desiderata piuttosto che l'end-effector. Ho anche un'altra domanda. Come potete vedere dalle immagini, nell'albero del modello simulink del robot, tra il sesto giunto e l'ultimo corpo ("EE'') ho posizionato un giunto saldato; l'ho fatto perché, altrimenti, nel programma viene letta una struttura del robot solo con sei corpi ed, inoltre, mostra solo il primo gruppo cinematco, terminando con il “Body6”. La domanda quindi è se è strettamente necessaria la presenza di un tipo di giunto prima dell'end-effector per visualizzare correttamente la struttura dell'albero in Matlab.
In conclusione, come posso eseguire l'IK applicato al mio robot ?
Grazie in anticipo per il vostro tempo, sperando di essere stato abbastanza chiaro.
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