Hello! I'm trying to solve the inverse kinematic using the Jacobian pseudo inverse for controlling my Kuka LWR4 (7 dof). I have calculated the matrix through a function, however when I run the script the robot doesn't behave as expected.
I think what's wrong is the Jacobian itself that is:
Code
void Jacobian(float *s, float *c, float J_inv[][6]){
float alpha[7] = {M_PI/2, -M_PI/2, -M_PI/2, M_PI/2, M_PI/2, -M_PI/2, 0};
float d[7] = {0,0,0.4,0,0.39,0,0.078};
float a[7] ={0,0,0,0,0,0,0};
float z_tool = 0.3;
//sin and cos according to the actual joint position
float s1 = s[0];
float c1 = c[0];
float s2 = s[1];
float c2 = c[1];
float s3 = s[2];
float c3 = c[2];
float s4 = s[3];
float c4 = c[3];
float s5 = s[4];
float c5 = c[4];
float s6 = s[5];
float c6 = c[5];
float s7 = s[6];
float c7 = c[6];
//lenght of the kuka arm
float d1 = 0.41;
float d2 = 0.39;
float d3 = 0.078 + 0.3;
//Jacobian
float J11 = -d2*(s4*(c1*s3+s1*c2*c3)-s1*c4*s2) - d3*(c6*(s4*(c1*s3+s1*c2*c3)-s1*c4*s2)-s6*(c5*(c4*(c1*s3+s1*c2*c3)+s1*s2*s4)+s5*(c3*c1-s1*c2*s3))) + d1*s1*s2;
float J12 = -d2*(s4*c1*s2*c3+c1*c4*c2) - d3*(c6*(s4*c1*s2*c3+c1*c4*c2)-s6*(c5*(c4*c1*s2*c3-c1*c2*s4)-s5*c1*s2*s3)) - d1*c1*c2 ;
float J13 = -d2*s4*(c3*s1+c1*c2*s3) - d3*(c6*s4*(c3*s1+c1*c2*s3)-s6*(c5*c4*(c3*s1+c1*c2*s3)+s5*(-s1*s3+c1*c2*c3)));
float J14 = -d2*(c4*(s1*s3-c1*c2*c3)-c1*s2*s4) - d3*(c6*(c4*(s1*s3-c1*c2*c3)-c1*s2*s4)-s6*c5*(-s4*(s1*s3-c1*c2*c3)-c1*c4*s2));
float J15 = d3*s6*(-s5*(c4*(s1*s3-c1*c2*c3)-c1*s2*s4)+c5*(c3*s1+c1*c2*s3));
float J16 = -d3*(-s6*(s4*(s1*s3-c1*c2*c3)+c1*c4*s2)-c6*(c5*(c4*(s1*s3-c1*c2*c3)-c1*s2*s4)+s5*(c3*s1+c1*c2*s3)));
float J17 = 0.0;
float J21 = d2*(s4*(-s1*s3+c1*c2*c3)-c1*c4*s2) + d3*(c6*(s4*(-s1*s3+c1*c2*c3)-c1*c4*s2)-s6*(c5*(c4*(-s1*s3+c1*c2*c3)+c1*s2*s4)+s5*(-c3*s1-c1*c2*s3))) - d1*c1*s2;
float J22 = d2*(-s2*s4*c3*s1-c4*s1*c2)+d3*(c6*(-s2*s4*c3*s1-c4*s1*c2)-s6*(c5*(-c4*s2*c3*s1+c2*s1*s4)+s5*s2*s1*s3)) - d1*s1*c2;
float J23 = d2*s4*(c1*c3-c2*s1*s3) + d3*(c6*s4*(c1*c3-c2*s1*s3)-s6*(c5*c4*(c1*c3-c2*s1*s3)+s5*(-c1*s3-c2*c3*s1)));
float J24 = d2*(c4*(c1*s3+c2*c3*s1)+s1*s2*s4) + d3*(c6*(c4*(c1*s3+c2*c3*s1)+s1*s2*s4)-s6*c5*(-s4*(c1*s3+c2*c3*s1)+c4*s1*s2));
float J25 = -d3*s6*(-s5*(c4*(c1*s3+c2*c3*s1)+s1*s2*s4)+c5*(c1*c3-c2*s1*s3));
float J26 = d3*(-s6*(s4*(c1*s3+c2*c3*s1)-c4*s1*s2)-c6*(c5*(c4*(c1*s3+c2*c3*s1)+s1*s2*s4)+s5*(c1*c3-c2*s1*s3)));
float J27 = 0.0;
float J31 = 0.0;
float J32 = d3*(s6*(c5*(-s2*s4-c3*c4*c2)+c2*s3*s5)+c6*(-s2*c4+c3*c2*s4)) + d2*(-s2*c4+c3*c2*s4) - d1*s2;
float J33 = d3*(s6*(c5*s3*c4*s2+s2*c3*s5)-c6*s3*s2*s4) - d2*s3*s2*s4;
float J34 = d3*(s6*c5*(c2*c4+c3*s2*s4)+c6*(-c2*s4+c3*c4*s2)) + d2*(-c2*s4+c3*c4*s2);
float J35 = d3*s6*(-s5*(c2*s4-c3*c4*s2)+s2*s3*c5);
float J36 = d3*(c6*(c5*(c2*s4-c3*c4*s2)+s2*s3*s5)-s6*(c2*c4+c3*s2*s4));
float J37 = 0.0;
float J41 = T_1_0[0][2]; //T_i_j transformation from FK<img src="https://www.robot-forum.com/images/smilies/robotforum/streicheln.gif" alt=":fine:" class="smiley" height="32" data-tooltip="n1" aria-label="n1" id="wscSmiley_0_0"><img src="https://www.robot-forum.com/images/smilies/robotforum/streicheln.gif" alt=":fine:" class="smiley" height="32" data-tooltip="n1" aria-label="n1" id="wscSmiley_0_1"><img src="https://www.robot-forum.com/images/smilies/robotforum/streicheln.gif" alt=":fine:" class="smiley" height="32" data-tooltip="n1" aria-label="n1" id="wscSmiley_0_2">
float J42 = T_2_0[0][2];
float J43 = T_3_0[0][2];
float J44 = T_4_0[0][2];
float J45 = T_5_0[0][2];
float J46 = T_6_0[0][2];
float J47 = T_7_0[0][2];
float J51 = T_1_0[1][2];
float J52 = T_2_0[1][2];
float J53 = T_3_0[1][2];
float J54 = T_4_0[1][2];
float J55 = T_5_0[1][2];
float J56 = T_6_0[1][2];
float J57 = T_7_0[1][2];
float J61 = T_1_0[2][2];
float J62 = T_2_0[2][2];
float J63 = T_3_0[2][2];
float J64 = T_4_0[2][2];
float J65 = T_5_0[2][2];
float J66 = T_6_0[2][2];
float J67 = T_7_0[2][2];
float J[6][7] = {{J11,J12,J13,J14,J15,J16,J17},{J21,J22,J23,J24,J25,J26,J27},{J13,J23,J33,J34,J35,J36,J37}, {J41,J42,J43,J44,J45,J46,J47}, {J51,J52,J53,J54,J55,J56,J57}, {J61,J62,J63,J64,J65,J66,J67}};
Display More
Do you happen to know what's wrong?
In any case, do you know if there is a simpler way to calculate it?