I think there should only be L, th2, th3 and th5 in J_home, am I missing something? I am trying to solve the determinant of J_home with the assume configuration
%finding Jacobian
syms t1 t2 t3 t31 t4 t41 t5 t51 t52 t6 te th1 th2 th3 th4 th5 th6 th1_ th2_ th3_ th4_ th5_ th6_ L t_1=pi/2 ;d_1=0;a_1=0;alpha_1=pi/2;t0=pi/2; d0=0;a0=0;alpha0=0;t1 = 0; d1 = th1_; a1 = 0; alpha1 = 0;t2 = th2_+pi; d2 = 0; a2 = L; alpha2 = 0; t3 = th3_-pi/2;d3 = 0; a3 = L; alpha3= -pi/2;t31 = 0; d31 =L; a31 = 0; alpha31 = 0;t4 = 0;d4 = L+th4_;a4 =-L; alpha4 = pi;t5 = th5_+pi;d5 = 0;a5 = L; alpha5 = pi/2;t51 = pi/2; d51= L; a51 = 0; alpha51 = pi/2;t52 = 0; d52 = L; a52 =0; alpha52 = 0;t6 = th6_;d6 = L;a6 = 0; alpha6 = 0; A_1_ = inTransMat(a_1,alpha_1,d_1,t_1); A0_ = inTransMat(a0,alpha0,d0,t0); %setting frame 0 as shown
A1 = inTransMat(a1,alpha1,d1,t1); A2_ = inTransMat(a2,alpha2,d2,t2); A3_ = inTransMat(a3,alpha3,d3,t3); A31_= inTransMat (a31,alpha31,d31,t31); A4_ = inTransMat(a4,alpha4,d4,t4); A5_= inTransMat(a5,alpha5,d5,t5); A51_= inTransMat (a51,alpha51,d51,t51); A52_= inTransMat (a52,alpha52,d52,t52); A6_= inTransMat(a6,alpha6,d6,t6); T_ = A_1_*A0_*A1*A2_*A3_*A31_*A4_*A5_*A51_*A52_*A6_ T01_ = A_1_*A0_*A1; T02_ = A_1_*A0_*A1*A2_; T03_ = A_1_*A0_*A1*A2_*A3_*A31_; T04_ = A_1_*A0_*A1*A2_*A3_*A31_*A4_; T05_ = A_1_*A0_*A1*A2_*A3_*A4_*A5_*A51_*A52_; T_06= vpa(T_,2) Z0= vpa(T01_(1:3,3),2) Z1= vpa(T01_(1:3,3),2) Z2= vpa(T02_(1:3,3),2) Z3= vpa(T03_(1:3,3),2) Z4= vpa(T04_(1:3,3),2) Z5= vpa(T05_(1:3,3),2) pe= T_06(1:3,4:end) %derivative of EE with respected q
Jv1 = Z0;Jv2 = jacobian([pe],[th2_]);Jv3 = jacobian([pe],[th3_]);Jv4 = Z3;Jv5 = jacobian([pe],[th5_]);Jv6 = jacobian([pe],[d6]);Jw1 = [0 0 0]';Jw2 = Z1;Jw3 = Z2;Jw4 = [0 0 0]';Jw5 = Z4;Jw6 = Z5;%JOINT 1,4 is a prismetic joint
%combine Jv and Jw into on column
J1q = [Jv1(:);Jw1(:)];J2q = [Jv2(:);Jw2(:)];J3q = [Jv3(:);Jw3(:)];J4q = [Jv4(:);Jw4(:)];J5q = [Jv5(:);Jw5(:)];J6q = [Jv6(:);Jw6(:)];J = [J1q J2q J3q J4q J5q J6q]J_home = vpa(J,2) %end effector @ homeposition
L=100;th2_=0;th3_=0;th5_=0;%assume the following configureation
J_fcn = matlabFunction(J_home)J_num = J_fcn(L,th2_,th3_,th5_)J_det=det(J_num)%matrix not in full rank
function IT = inTransMat(a,b,c,d)IT = [cos(d) -sin(d)*cos(b) sin(d)*sin(b) a*cos(d); sin(d) cos(d)*cos(b) -cos(d)*sin(b) a*sin(d); 0 sin(b) cos(b) c; 0 0 0 1];endfunction T = TransMat(a,b,c,d)T = [cos(d) -sin(d)*cos(b) sin(d)*sin(b) a*cos(d); sin(d) cos(d)*cos(b) -cos(d)*sin(b) a*sin(d); 0 sin(b) cos(b) c; 0 0 0 1];end
Best Answer