Is there a way to make q_dot to show numeric solutions?
syms th1 th2 th3 th4 th5 th6 d1 d2 d3 d4 d5 d6 alpha1 alpha2 alpha3 alpha4 alpha5 alpha6 a1 a2 a3 a4 a5 a6 d1 d2 d3 d4 d5 d6 q_dot Js%DH parameters
%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= A01*A12A03= A01*A12*A23%A04= A01*A12*A23*A34
%A05= A01*A12*A23*A34*A45
%A06= 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]'
pe=A03(1:3,4:end);%derivative of EE with respected q
Jv1 = jacobian([pe],[th1]);Jv2 = jacobian([pe],[th2]);Jv3 = jacobian([pe],[th3]);%Jv4 = simplify(jacobian([pe],[th4]));
%Jv5 = simplify(jacobian([pe],[th5]));
%Jv6 = simplify(jacobian([pe],[d6]));
Jw1 = Z0;Jw2 = Z1;Jw3 = Z2;%Jw4 = Z3;
%Jw5 = Z5;
%Jw6 is a prismetic joint
%Jw6 = [0 0 0]'
%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))
x = [5 5 10]'J3 = J(1:3,:)%extract J3 first 3 rows, all columns)
th1 = 1.3734th2 = -0.4038th3 = 1.4330d1 = 475;d2 = 0; d3 = 0 ;alpha1 = pi/2alpha2 = 0; alpha3 = 0;d4 =0 ;d5 = 0;d6 =85;a1 = 150;a2 = 600;a3 = sqrt(720^2+120^2);% input
x = [5 5 10]'q_dot = inv(J3)*x
Best Answer