Izračunal bi Jacobijevo matriko za določenega robota. Stanje je takšno da mi posamezni členi matrike (torej: A, B, C, D, E, F, G, H, I), vrnejo vektor.
Nisem najboljši matematik, ampak se trudim po najboljših močeh. Prosil bi Vas za pomoč, oziroma pregled matlabove kode, če je pravilna.
Namreč kaj počne koda: Dobi neke vrednosti zunanjih koordinat, od kje do kje naj izračuna gibljivostni indeks robota. Nato pa te vrednosti postavi v tri stolpce, ter izračuna za posamezno točko x, y, z gibljivosti indeks (oziroma ga naj bi). Zaplete pa se pri Jacobijanovi matriki.
PROSIM LEPO ZA POMOČ!
Koda je naslednja:
Koda: Izberi vse
% dimenzije robota%
a1 = 250;
d1 = 645;
a2 = 1000;
a3 = 230;
d2 = 1587.4;
% branje minimalne vrednosti
x_min = 2;
y_min = 3;
z_min = 4;
% skaliranje vrednosti
x_res = 1;
y_res = 1;
z_res = 1;
% branje maksimalne vrednosti
x_max = 3;
y_max = 4;
z_max = 5;
% grupiranje podatkov v točke
x = [x_min:x_res:x_max]';
y = [y_min:y_res:y_max]';
z = [z_min:z_res:z_max]';
% izračun notranjih koordinat
q1 = atan2(y,x);
S2 = sin(a2);
C2 = cos(a2);
q2 = atan2(S2,C2);
S3 = sin(a3);
C3 = cos(a3);
B = d2*S3 - a3*C3;
SB = sqrt(-(B*B)+a3*a3+d2*d2);
q3 = atan2(a3,d2)-atan2(B, SB);
A = (-d2*cos(q1)*cos(q2)*sin(q3)-d2*cos(q1)*sin(q2)*cos(q3)-a3*cos(q1)*cos(q2)*cos(q3)+a3*cos(q1)*sin(q2)*sin(q3)-a2*cos(q1)*cos(q2)+a1*sin(q1)); % "levi zgoraj"
B = (d2*sin(q1)*sin(q2)*sin(q3)-d2*sin(q1)*cos(q2)*cos(q3)+a3*sin(q1)*sin(q2)*cos(q3)+a3*sin(q1)*cos(q2)*sin(q3)+a2*sin(q1)*sin(q2)+a1*cos(q1)); % "sredina zgoraj"
C = (-d2*sin(q1)*cos(q2)*cos(q3)+d2*sin(q1)*sin(q2)*sin(q3)+a3*sin(q1)*cos(q2)*sin(q3)+a3*sin(q1)*sin(q2)*cos(q3)-a2*sin(q1)*cos(q2)+a1*cos(q1)); % "desno zgoraj"
D = (-d2*sin(q1)*cos(q2)*sin(q3)-d2*sin(q1)*sin(q2)*cos(q3)-a3*sin(q1)*cos(q2)*cos(q3)+a3*sin(q1)*sin(q2)*sin(q3)-a3*sin(q1)*cos(q2)+a3*cos(q1)); % "levi v sredini"
E = (-d2*cos(q1)*sin(q2)*sin(q3)+d2*cos(q1)*cos(q2)*cos(q3)-a3*cos(q1)*sin(q2)*cos(q3)-a3*cos(q1)*cos(q2)*sin(q3)-a3*cos(q1)*sin(q2)+a3*sin(q1)); % "sredina v sredini"
F = (d2*cos(q1)*cos(q2)*cos(q3)-d2*cos(q1)*sin(q2)*sin(q3)-a3*cos(q1)*cos(q2)*sin(q3)-a3*cos(q1)*sin(q2)*cos(q3)+a3*cos(q1)*cos(q2)+a3*sin(q1)); % "desni v sredini"
G = (d2*sin(q2)*sin(q3)+d2*cos(q2)*cos(q3)+a3*sin(q2)*cos(q3)+a3*cos(q2)*sin(q3)+a2*sin(q2)+d1); % "levi spodaj"
H = (d2*cos(q2)*sin(q3)+d2*sin(q2)*cos(q3)+a3*cos(q2)*cos(q3)-a3*sin(q2)*sin(q3)+a2*cos(q2)+d1); % "sredinski spodaj"
I = (d2*sin(q2)*cos(q3)+d2*cos(q2)*sin(q3)-a3*sin(q2)*sin(q3)+a3*cos(q2)*cos(q3)+a2*sin(q2)+d1); % "desni spodaj"
Jac =[A, B, C; D, E, F; G, H, I];
% izračun determinante matrike
detJac = det(Jac);
% izračun gibljivostnega indeksa
w = sqrt(detJac*(Jac'));