Program

Program izdelan v programskem jeziku Mathematica izracuna kinematiko robota PUMA.

Needs["Geometry`Robotics`"];
A1 = HDHMatrix[theta1, 0, 0, -Pi/2];
A2 = HDHMatrix[theta2, d2, a2, 0];
A3 = HDHMatrix[theta3 + Pi/2, 0, -a3, Pi/2];
A4 = HDHMatrix[theta4, d4, 0, -Pi/2];
A5 = HDHMatrix[theta5, 0, 0, Pi/2];
A6 = HDHMatrix[theta6, d6, 0, 0];

T60 = A1.A2.A3.A4.A5.A6;
T61 = A2.A3.A4.A5.A6;
T62 = A3.A4.A5.A6;
T63 = A4.A5.A6;
T64 = A5.A6;
T65 = A6;


Jaco[x_] := Block[ {}, 
nx = x[[1,1]]; ox = x[[1,2]]; ax = x[[1,3]]; px = x[[1,4]]; 
ny = x[[2,1]]; oy = x[[2,2]]; ay = x[[2,3]]; py = x[[2,4]]; 
nz = x[[3,1]]; oz = x[[3,2]]; az = x[[3,3]]; pz = x[[3,4]];
Q = {};
tdx = px * ny - py * nx;
tdy = oy * px - ox * py;
tdz = ay * px - ax * py;
ddx = nz;
ddy = oz;
ddz = az;

Q = Append[Q, tdx];
Q = Append[Q, tdy];
Q = Append[Q, tdz];
Q = Append[Q, ddx];
Q = Append[Q, ddy];
Q = Append[Q, ddz];
Q = SimplifyTrig[Expand[Q]]
]

Print["Podmatrike ..."];

J1v = Jaco[T60];
J2v = Jaco[T61];
J3v = Jaco[T62];
J4v = Jaco[T63];
J5v = Jaco[T64];
J6v = Jaco[T65];

Print["Transponiram vrstice v stolpce..."];

J1 = Transpose[{J1v}];
J2 = Transpose[{J2v}];
J3 = Transpose[{J3v}];
J4 = Transpose[{J4v}];
J5 = Transpose[{J5v}];
J6 = Transpose[{J6v}];

Print["Sestavljam Jacobi matriko..."];
J = {};
J = Append[J, J1];
J = Append[J, J2];
J = Append[J, J3];
J = Append[J, J4];
J = Append[J, J5];
J = Append[J, J6];

Print["Poenostavljam Jacobi matriko ..."];

J = SimplifyTrig[J];

Print["Izpis"];

Save["j1.prn", J1];
Save["j2.prn", J2];
Save["j3.prn", J3];
Save["j4.prn", J4];
Save["j5.prn", J5];
Save["j6.prn", J6];
Save["j.prn", J];
 
J1t = TeXForm[J1];
J2t = TeXForm[J2];
J3t = TeXForm[J3];
J4t = TeXForm[J4];
J5t = TeXForm[J5];
J6t = TeXForm[J6];
Jt  = TeXForm[J]

Save["j1.TeX", J1t];
Save["j2.TeX", J2t];
Save["j3.TeX", J3t];
Save["j4.TeX", J4t];
Save["j5.TeX", J5t];
Save["j6.TeX", J6t];
Save["j.TeX", Jt];

Print["Konec"];