Looking for idea to simplify, A06 as well as J06.. any thougths?
syms th th1 th2 th3 th4 th5 th6 alpha1 alpha2 alpha3 alpha4 alpha5 alpha6 a1 a2 a3 a4 a5 a6 d1 d2 d3 d4 d5 d6 c1 c2 c3 c4 c5 c6 s1 s2 s3 s4 s5 s6 N%DH parameters
%d1 = 475;
%d2 = 0;
%d3 = 0 ;
%d4 =0 ;
%d5 = 0;
%d6 =85;
%a1 = 150;
%a2 = 600;
%a3 = sqrt(720^2+120^2);
%a4 = 0
%a5 = 0
%a6 = 0
% Forward Kinematics
A01 = [cos(th1) -cos(alpha1)*sin(th1) sin(alpha1)*sin(th1) a1*cos(th1);sin(th1) cos(alpha1)*cos(th1) -sin(alpha1)*cos(th1) a1*sin(th1);0 sin(alpha1) cosd(alpha1) d1;0 0 0 1];A12 = [cos(th2) -cos(alpha2)*sin(th2) sin(alpha2)*sin(th2) a2*cos(th2);sin(th2) cos(alpha2)*cos(th2) -sin(alpha2)*cos(th2) a2*sin(th2);0 sin(alpha2) cos(alpha2) d2;0 0 0 1];A23 = [cos(th3) -cos(alpha3)*sin(th3) sin(alpha3)*sin(th3) a3*cos(th3);sin(th3) cos(alpha3)*cos(th3) -sin(alpha3)*cos(th3) a3*sin(th3);0 sind(alpha3) cos(alpha3) d3;0 0 0 1];A34 = [cos(th4) -cos(alpha4)*sin(th4) sin(alpha4)*sin(th4) a4*cos(th4);sin(th4) cos(alpha4)*cos(th4) -sin(alpha4)*cos(th4) a4*sin(th4);0 sin(alpha4) cos(alpha4) d4;0 0 0 1];A45 = [cos(th5) -cos(alpha5)*sin(th5) sin(alpha5)*sin(th5) a5*cos(th5);sin(th5) cos(alpha5)*cos(th5) -sin(alpha5)*cos(th5) a5*sin(th5);0 sin(alpha5) cos(alpha5) d5;0 0 0 1];A56 = [cos(th6) -cos(alpha6)*sin(th6) sin(alpha6)*sin(th6) a6*cos(th6);sin(th6) cos(alpha6)*cos(th6) -sin(alpha6)*cos(th6) a6*sin(th6);0 sind(alpha6) cos(alpha6) d6;0 0 0 1];A01A02= simplify(A01*A12)A03= simplify(A01*A12*A23)A04= simplify(A01*A12*A23*A34)A05= simplify(A01*A12*A23*A34*A45)A06= simplify(A01*A12*A23*A34*A45*A56)%last column of the Rotation matrix
Z0=[0 0 1]'Z1=[0 -1 0]'Z2=[0 -1 0]'Z3=[0 -1 0]'Z4=[0 0 -1]'Z5=[1 0 0]'Z6=[1 0 0]'pe=A06(1:3,4:end);%derivative of EE with respected q
Jv1 = jacobian([pe],[th1]);Jv2 = jacobian([pe],[th2]);Jv3 = jacobian([pe],[th3]);Jv4 = jacobian([pe],[th4]);Jv5 = jacobian([pe],[th5]);Jv6 = jacobian([pe],[th6]);Jw1 = Z0;Jw2 = Z1;Jw3 = Z2;Jw4 = Z3;Jw5 = Z5;Jw6 = Z6;%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]%cross producr method
%p0=[0 0 0]'
%p1=A01(1:3,4:end);
%p2=A02(1:3,4:end);
%p3=A03(1:3,4:end);
%p4=A04(1:3,4:end);
%p5=A05(1:3,4:end);
%pe=A06(1:3,4:end);
%pe10=simplify(pe-p0)
%pe20=simplify(pe-p1)
%pe30=simplify(pe-p2)
%pe40=simplify(pe-p3)
%pe50=simplify(pe-p4)
%pe60=simplify(pe-p5)
%Jv1=simplify(cross(Z0,pe10))
%Jv2=simplify(cross(Z1,pe20))
%Jv3=simplify(cross(Z2,pe30))
%Jv4=simplify(cross(Z3,pe40))
%Jv5=simplify(cross(Z4,pe50))
%Jv6=simplify(cross(Z5,pe60))
Best Answer