The following is the simplified C code for the Jacobian of RRP:RRR manipulator. The Jacobian is derived by using Maple math engine. All the steps of derivation are listed. (The rest of Appendix B is not included)
This is Jacobian for RRP : RRR
file name : JRRP.mws
a1:=a1;
a2:=a2;
a3:=a3;
a4:=0;
a5:=0;
a6:=0;
d1:=d1;
d2:=d2;
d3:=d3;
d4:=0;
d5:=0;
d6:=d6;
alpha1:=alpha1;
alpha2:=alpha2;
alpha3:=alpha3;
alpha4:=-Pi/2;
alpha5:=Pi/2;
alpha6:=0;
theta1:=theta1;
theta2:=theta2;
theta3:=theta3;
theta4:=theta4;
theta5:=theta5;
theta6:=theta6;
AA:=evalm(A1&*A2&*A3&*A4&*A5&*A6):
AA1:=A1:
AA2:=evalm(A1&*A2):
AA3:=evalm(A1&*A2&*A3):
AA4:=evalm(A1&*A2&*A3&*A4):
AA5:=evalm(A1&*A2&*A3&*A4&*A5):
AA6:=evalm(A1&*A2&*A3&*A4&*A5&*A6):
Z0:=vector([0,0,1]):
Z1:=vector([AA1[1,3],AA1[2,3],AA1[3,3]]):
Z2:=vector([AA2[1,3],AA2[2,3],AA2[3,3]]):
Z3:=vector([AA3[1,3],AA3[2,3],AA3[3,3]]):
Z4:=vector([AA4[1,3],AA4[2,3],AA4[3,3]]):
Z5:=vector([AA5[1,3],AA5[2,3],AA5[3,3]]):
O0:=vector([0,0,0]):
O1:=vector([AA1[1,4],AA1[2,4],AA1[3,4]]):
O2:=vector([AA2[1,4],AA2[2,4],AA2[3,4]]):
O3:=vector([AA3[1,4],AA3[2,4],AA3[3,4]]):
O4:=vector([AA4[1,4],AA4[2,4],AA4[3,4]]):
O5:=vector([AA5[1,4],AA5[2,4],AA5[3,4]]):
O6:=vector([AA6[1,4],AA6[2,4],AA6[3,4]]):
O60:=evalm(O6-O0):
O61:=evalm(O6-O1):
O62:=evalm(O6-O2):
O63:=evalm(O6-O3):
O64:=evalm(O6-O4):
O65:=evalm(O6-O5):
CP0:=crossprod(Z0,O60):
CP1:=crossprod(Z1,O61):
CP2:=crossprod(Z2,O62):
CP3:=crossprod(Z3,O63):
CP4:=crossprod(Z4,O64):
CP5:=crossprod(Z5,O65):
J:=matrix([[CP0[1],CP1[1],Z2[1],CP3[1],CP4[1]
,CP5[1]],[CP0[2],CP1[2],Z2[2],CP3[2],CP4[2],CP5[2]],[CP0[3],CP1[3],Z2[
3],CP3[3],CP4[3],CP5[3]],[Z0[1],Z1[1],0,Z3[1],Z4[1],Z5[1]],[Z0[2],Z1[2
],0,Z3[2],Z4[2],Z5[2]],[Z0[3],Z1[3],0,Z3[3],Z4[3],Z5[3]]]):
J11:=J[1,1];
J12:=J[1,2];
J13:=J[1,3];
J14:=J[1,4];
J15:=J[1,5];
J16:=J[1,6];
J21:=J[2,1];
J22:=J[2,2];
J23:=J[2,3];
-
-
-
-
-
-
-
J65:=J[6,5];
{J66:=J[6,6];
Then Jacobian is converted to a C Code
C(J,optimized);
t1 = sin(theta1);
t2 = cos(theta2);
t4 = cos(theta1);
t5 = cos(alpha1);
t6 = t4*t5;
t7 = sin(theta2);
t9 = t1*t2+t6*t7;
t10 = cos(theta3);
t12 = t1*t7;
t13 = cos(alpha2);
t15 = t2*t13;
t17 = sin(alpha1);
t18 = t4*t17;
t19 = sin(alpha2);
t21 = -t12*t13+t6*t15-t18*t19;
t22 = sin(theta3);
t24 = t9*t10+t21*t22;
t25 = cos(theta4);
t27 = t9*t22;
t28 = cos(alpha3);
t30 = t21*t10;
t33 = t2*t19;
t36 = t12*t19-t6*t33-t18*t13;
t37 = sin(alpha3);
t39 = -t27*t28+t30*t28+t36*t37;
t40 = sin(theta4);
t43 = sin(theta5);
t45 = t27*t37;
t46 = t30*t37;
t47 = t36*t28;
t49 = cos(theta5);
t51 = (t24*t25+t39*t40)*t43-(-t45+t46-t47)*t49;
t52 = t51*d6;
t54 = t9*a3*t10;
t56 = t21*a3*t22;
t57 = t36*d3;
t59 = t1*a2*t2;
t60 = a2*t7;
t61 = t6*t60;
t62 = t18*d2;
t65 = t17*t7;
t67 = t17*t2;
t70 = t67*t13+t5*t19;
t72 = t65*t10+t70*t22;
t76 = t70*t10;
t80 = -t67*t19+t5*t13;
t82 = -t65*t22*t28+t76*t28+t80*t37;
t87 = t65*t22*t37;
t88 = t76*t37;
t89 = t80*t28;
t92 = (t72*t25+t82*t40)*t43-(-t87+t88-t89)*t49;
t102 = t92*d6+t65*a3*t10+t70*a3*t22+t80*d3+t17*a2*t7+t5*d2;
t104 = t52+t54+t56+t57+t59+t61-t62;
t107 = t4*t7;
t109 = t1*t5;
t111 = t1*t17;
t113 = t107*t19+t109*t33+t111*t13;
t114 = t45-t46+t47;
t117 = t87-t88+t89;
t123 = -t24*t40+t39*t25;
t128 = -t72*t40+t82*t25;
t134 = t4*t2-t109*t7;
t139 = -t107*t13-t109*t15+t111*t19;
t141 = t134*t10+t139*t22;
t143 = t134*t22;
t145 = t139*t10;
t148 = -t143*t28+t145*t28+t113*t37;
t152 = t143*t37;
t153 = t145*t37;
t154 = t113*t28;
t157 = (t141*t25+t148*t40)*t43-(-t152+t153-t154)*t49;
t158 = t157*d6;
t160 = t134*a3*t10;
t162 = t139*a3*t22;
t163 = t113*d3;
t165 = t4*a2*t2;
t166 = t109*t60;
t167 = t111*d2;
t170 = t158+t160+t162+t163+t165-t166+t167;
t176 = t152-t153+t154;
t184 = -t141*t40+t148*t25;
J[0][0] = -t52-t54-t56-t57-t59-t61+t62-a1*t1;
J[0][1] = -t18*t102-t5*t104;
J[0][2] = t113;
J[0][3] = t114*t92*d6-t117*t51*d6;
J[0][4] = t123*t92*d6-t128*t51*d6;
J[0][5] = 0.0;
J[1][0] = t158+t160+t162+t163+t165-t166+t167+a1*t4;
J[1][1] = t5*t170-t111*t102;
J[1][2] = t36;
J[1][3] = t117*t157*d6-t176*t92*d6;
J[1][4] = t128*t157*d6-t184*t92*d6;
J[1][5] = 0.0;
J[2][0] = 0.0;
J[2][1] = t111*t104+t18*t170;
J[2][2] = t80;
J[2][3] = t176*t51*d6-t114*t157*d6;
J[2][4] = t184*t51*d6-t123*t157*d6;
J[2][5] = 0.0;
J[3][0] = 0.0;
J[3][1] = t111;
J[3][2] = 0.0;
J[3][3] = t176;
J[3][4] = t184;
J[3][5] = t157;
J[4][0] = 0.0;
J[4][1] = -t18;
J[4][2] = 0.0;
J[4][3] = t114;
J[4][4] = t123;
J[4][5] = t51;
J[5][0] = 1.0;
J[5][1] = t5;
J[5][2] = 0.0;
J[5][3] = t117;
J[5][4] = t128;
J[5][5] = t92;
The folloeing is the Symbolic Inverse Jacobian. All the program do is to substitute the values of the elements of the jacobian matrix as a real numbers into the Inverse Jacobain.
This is Jacobian for RRP : RRR
file name : JRRP.mws
with(linalg):
a1:=a1;
a2:=a2;
a3:=a3;
a4:=0;
a5:=0;
a6:=0;
d1:=d1;
d2:=d2;
d3:=d3;
d4:=0;
d5:=0;
d6:=d6;
alpha1:=0;
alpha2:=0;
alpha3:=0;
alpha4:=-Pi/2;
alpha5:=Pi/2;
alpha6:=0;
theta1:=theta1;
theta2:=theta2;
theta3:=theta3;
theta4:=theta4;
theta5:=theta5;
theta6:=theta6;
AA:=evalm(A1*A2*A3*A4*A5*A6)
AA1:=A1:
AA2:=evalm(A1*A2):
AA3:=evalm(A1*A2*A3):
AA4:=evalm(A1*A2*A3*A4):
AA5:=evalm(A1*A2*A3*A4*A5):
AA6:=evalm(A1*A2*A3*A4*A5*A6):
Z0:=vector([0,0,1]):
Z1:=vector([AA1[1,3],AA1[2,3],AA1[3,3]]):
Z2:=vector([AA2[1,3],AA2[2,3],AA2[3,3]]):
Z3:=vector([AA3[1,3],AA3[2,3],AA3[3,3]]):
Z4:=vector([AA4[1,3],AA4[2,3],AA4[3,3]]):
Z5:=vector([AA5[1,3],AA5[2,3],AA5[3,3]]):
O0:=vector([0,0,0]):
O1:=vector([AA1[1,4],AA1[2,4],AA1[3,4]]):
O2:=vector([AA2[1,4],AA2[2,4],AA2[3,4]]):
O3:=vector([AA3[1,4],AA3[2,4],AA3[3,4]]):
O4:=vector([AA4[1,4],AA4[2,4],AA4[3,4]]):
O5:=vector([AA5[1,4],AA5[2,4],AA5[3,4]]):
O6:=vector([AA6[1,4],AA6[2,4],AA6[3,4]]):
O60:=evalm(O6-O0):
O61:=evalm(O6-O1):
O62:=evalm(O6-O2):
O63:=evalm(O6-O3):
O64:=evalm(O6-O4):
O65:=evalm(O6-O5):
CP0:=crossprod(Z0,O60):
CP1:=crossprod(Z1,O61):
CP2:=crossprod(Z2,O62):
CP3:=crossprod(Z3,O63):
CP4:=crossprod(Z4,O64):
CP5:=crossprod(Z5,O65):
readlib(C):
readlib(C);
t1 = cos(theta1);
t2 = cos(theta2);
t3 = t1*t2;
t4 = cos(theta3);
t6 = t3*a3*t4;
t7 = t1*a2;
t8 = t7*t2;
t9 = t1*a3;
t10 = sin(theta3);
t11 = sin(theta2);
t12 = t10*t11;
t13 = t9*t12;
t14 = a3*t10;
t15 = sin(theta1);
t16 = t15*t2;
t17 = t14*t16;
t18 = a3*t15;
t19 = t11*t4;
t20 = t18*t19;
t21 = t15*a2;
t22 = t21*t11;
t23 = t6+t8-t13-t17-t20-t22;
t24 = t1*t1;
t25 = t24*a3;
t26 = t10*t2;
t29 = t24*a2;
t30 = t29*t11;
t31 = t15*t15;
t32 = t31*a3;
t35 = t31*a2;
t36 = t35*t11;
t38 = 1/(t25*t26+t25*t19+t30+t32*t26+t32*t19+t36);
t40 = 1/a1;
t42 = t9*t26;
t43 = t9*t19;
t44 = t7*t11;
t47 = t2*a3*t15*t4;
t48 = t21*t2;
t49 = t15*t11;
t50 = t14*t49;
t51 = t42+t43+t44+t47+t48-t50;
t55 = cos(theta5);
t57 = t55*t38*t40;
t61 = t2*t2;
t62 = t61*a3;
t63 = sin(theta4);
t64 = t4*t4;
t65 = t63*t64;
t66 = t62*t65;
t67 = t61*a2;
t68 = t63*t4;
t69 = t67*t68;
t70 = t10*t10;
t72 = t62*t70*t63;
t73 = cos(theta4);
t74 = t73*t10;
t75 = t67*t74;
t76 = t11*t11;
t78 = a3*t76*t65;
t80 = t76*t63;
t81 = t4*a2*t80;
t84 = t63*a3*t76*t70;
t85 = a2*t76;
t86 = t85*t74;
t88 = sin(theta5);
t90 = t14*t2;
t92 = a3*t11*t4;
t93 = a2*t11;
t95 = 1/(t90+t92+t93);
t97 = d6*t95*t40;
t100 = t6+t8+a1*t1-t13-t17-t20-t22;
t104 = t42+t43+t44+t47+t48-t50+a1*t15;
t111 = t2*t4;
t112 = t63*a1;
t114 = a1*t73;
t118 =
t66+t69+t72+t75+t111*t112+t26*t114+t78+t81+t19*t114-t12*t112+t84+
t86;
t121 = t73*t4;
t123 = t1*t11;
t129 = t63*t10;
t132 =
t121*t16+t121*t123+t74*t3-t74*t49+t68*t3-t68*t49-t129*t123-t129*
t16;
t143 =
-t129*t3+t121*t3-t74*t123-t68*t123-t74*t16-t68*t16+t129*t49-t121*
t49;
t149 = t15*t88*d6;
t150 = t73*t73;
t151 = t150*t64;
t152 = t151*t76;
t154 = t68*t11;
t158 = t74*t11;
t162 = t151*t61;
t164 = t63*t63;
t165 = t164*t64;
t166 = t165*t76;
t168 = t165*t61;
t170 = a3*t70;
t171 = t61*t73;
t174 = t18*t4;
t175 = t2*t73;
t176 = t175*t12;
t178 = t9*t4;
t179 = t11*t63;
t180 = t179*t26;
t182 = t76*t73;
t183 = t182*t10;
t185 = t164*t70;
t186 = t185*t76;
t188 = t9*t70;
t189 = t175*t11;
t191 = t9*t10;
t192 = t171*t4;
t194 =
-t149*t152-t48*t154-t85*t121*t15-t48*t158+t85*t129*t15-t149*t162-
t149*t166-t149*t168-t170*t171*t15-2.0*t174*t176-2.0*t178*t180-t178*t18
3-t149*
t186-t188*t189+t191*t192;
t195 = a3*t64;
t198 = t18*t70;
t199 = t179*t2;
t201 = t18*t64;
t203 = t150*t70;
t204 = t203*t76;
t207 = t61*t63*t10;
t209 = t64*t76;
t212 = t185*t61;
t214 = t203*t61;
t216 = t80*t4;
t218 = t9*t64;
t221 = t121*t2;
t223 = t129*t2;
t225 = t18*t10;\
t227 = t70*t61;
t230 =
-t195*t182*t15+t198*t199-t201*t199-t149*t204-t174*t207-t9*t209*t63
-t149*t212-t149*t214-t7*t216+t218*t189-t7*t183+t44*t221-t44*t223+t225*
t216-t9*
t227*t63;
t233 = t76*t11;
t234 = t233*t150;
t235 = t234*t64;
t239 = t64*t4;
t240 = t25*t239;
t241 = t11*t164;
t242 = t241*t61;
t244 = t25*t10;
t245 = t2*t150;
t246 = t245*t209;
t248 = t70*t10;
t249 = t25*t248;
t250 = t245*t76;
t252 = t61*t2;
t253 = t248*t252;
t254 = t253*t164;
t256 = t253*t150;
t258 = t239*t233;
t259 = t258*t164;
t261 = t25*t4;
t262 = t234*t70;
t264 = t258*t150;
t266 = t2*t164;
t267 = t266*t76;
t269 =
t35*t235+t36*t214+t36*t212+t240*t242+t244*t246+
t249*t250+t25*t254+
t25*t256+t25*t259+t261*t262
+t25*t264+t249*t267;
t270 = t11*t150;
t271 = t270*t61;
t274 = t252*t164*t64;
t276 = t266*t209;
t278 = t32*t4;
t281 = t241*t227;
t283 = t270*t227;
t286 = t32*t248;
t288 = t32*t10;
t290 = t252*t150*t64;
t294 =
t240*t271+t244*t274+t244*t276+
t278*t262+t32*t264+t278*t281+t278*
t283+t32*t254+t286*t250+t288*t290+
t32*t256+t288*t276;
t297 = t32*t239;
t300 = t233*t164;
t301 = t300*t70;
t306 = t300*t64;
t312 =
t36*t168+t297*t242+t29*t235+t29*t301+t30*t168+
t30*t214+t261*t281+
t29*t306+t30*t162+
t30*t212+t35*t306+t36*t162;
t325 =
t35*t301+t35*t262+t261*t301+t261*t283+t29*t262+
t288*t246+t288*t274
+t278*t301+t244*t290+t297*t271+t286*t267+t32*t259;
t329 = 1/t88;
t330 = 1/(t269+t294+t312+t325)*t329;
t341 = t1*t88*d6;
t351 =
-2.0*t174*t180-t21*t216-t21*t183+t22*t221+
t8*t154+t8*t158+t7*t182*
t4+t341*t186-t7*t80*t10+t341*t152-t22*t223+t341*t214+t341*t204+t341*t1
62+t178*
t207;
t373 =
t341*t166+t341*t212-t195*t15*t76*t63-t198*t189-t170*t15*t61*t63-
t174*t183+t9*t227*t73+t225*t192+t341*t168+t9*t209*t73-t191*t216+t218*t
199+t201*
t189+2.0*t178*t176-t188*t199;
t377 = t88*d6;
t378 = t377*t63;
t380 = t377*t73;
t386 = t24*t76;
t388 = t31*t61;
t390 = t24*t61;
t393 = t31*t76;
t406 =
t185*t386+t185*t388+t203*t390+t151*t386+t203*t393+t151*t388+t165*
t390+t165*t393+t185*t390+t203*t386+t151*t393+t151*t390+t165*t386+t165*
t388+t185
*t393+t203*t388;
t407 = 1/t406;
t408 = t132*t407;
t409 = t143*t407;
Inverse_Jacobian[0][0] = t23*t38*t40;
Inverse_Jacobian[0][1] = t51*t38*t40;
Inverse_Jacobian[0][2] = 0.0;
Inverse_Jacobian[0][3] = t51*d6*t57;
Inverse_Jacobian[0][4] = -t23*d6*t57;
Inverse_Jacobian[0][5] =
(t66+t69+t72+t75+t78+t81+t84+t86)*t88*t97;
Inverse_Jacobian[1][0] = -t100*t38*t40;
Inverse_Jacobian[1][1] = -t104*t38*t40;
Inverse_Jacobian[1][2] = 0.0;
Inverse_Jacobian[1][3] = -t104*d6*t57;
Inverse_Jacobian[1][4] = t100*d6*t57;
\
Inverse_Jacobian[1][5] = -t118*t88*t97;
Inverse_Jacobian[2][0] = 0.0;
Inverse_Jacobian[2][1] = 0.0;
Inverse_Jacobian[2][2] = 1.0;
Inverse_Jacobian[2][3] = -t132*t88*d6;
Inverse_Jacobian[2][4] = t143*t88*d6;
Inverse_Jacobian[2][5] = 0.0;
Inverse_Jacobian[3][0] = t1*t38;
Inverse_Jacobian[3][1] = t15*t38;
Inverse_Jacobian[3][2] = 0.0;
Inverse_Jacobian[3][3] = -(t194+t230)*t55*t330;
Inverse_Jacobian[3][4] = -(t351+t373)*t55*t330;
Inverse_Jacobian[3][5] =
(t111*t378+t26*t380+t90+t92+t19*t380-t12*t378+
t93)*t95;
Inverse_Jacobian[4][0] = 0.0;
Inverse_Jacobian[4][1] = 0.0;
Inverse_Jacobian[4][2] = 0.0;
Inverse_Jacobian[4][3] = -t408;
Inverse_Jacobian[4][4] = t409;
Inverse_Jacobian[4][5] = 0.0;
Inverse_Jacobian[5][0] = 0.0;
Inverse_Jacobian[5][1] = 0.0;
Inverse_Jacobian[5][2] = 0.0;
Inverse_Jacobian[5][3] = t409*t329;
Inverse_Jacobian[5][4] = t408*t329;
Inverse_Jacobian[5][5] = 0.0;
