need help on nomenclature, Simplify symbolic variables

1 view (last 30 days)
Tony Yu on 30 Jun 2020
Answered: Star Strider on 30 Jun 2020
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];
A01
A02= 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))

Star Strider on 30 Jun 2020
If simplify is not simplifying to the extent that you want it to, use the Steps name-value pair to tell it to keep simplifying until it cannot simplify further or encounters the iteration limit. (I usually use 'Steps',500 since that most often works.)