problem in making Two link manipulator used inverse kinematics(jacobian)

Question

0 votes

i dont know what is wrong...

i used jacobian driven DHtable

i think animation part is not wrong

but find jacobian part has matter but i cant find that...

clc, clear, close all;
syms theta1 theta2 t
%% init
l1 = 1 ; l2 = 1;
d1 = 0; a1 = l1 ;alpha1 = 0; home1 = -60;
d2 = 0; a2 = l2 ;alpha2 = 0; home2 = 120;
%% find jacobian
A01 = DHtheta(theta1, d1, a1, alpha1,home1);
A12 = DHtheta(theta2, d2, a2, alpha2,home2);
T01 = A01;
T02 = simplify(A01*A12);
O02 = T02(1:3,4);
O01 = T01(1:3,4);
z = [0;0;1];
Jv1 = simplify(cross(z,O02));
Jv2 = simplify(cross(z,(O02-O01)));
J = [Jv1,Jv2;z,z];
%% trajectory planning
x0 = cosd(home1)+cosd(home1+home2);
y0 = sind(home1)+sind(home1+home2);
ax = amat(x0,0,0,x0,0,0,0,1);
ay = amat(y0,0,0,1,0,0,0,1);
xtraj = ax(1)*t.^5+ax(2)*t.^4+ax(3)*t.^3+ax(4)*t.^2+ax(5)*t+ax(6);
ytraj = ay(1)*t.^5+ay(2)*t.^4+ay(3)*t.^3+ay(4)*t.^2+ay(5)*t+ay(6);
xdottmp = diff(xtraj,t);
ydottmp = diff(ytraj,t);
treal = linspace(0,1,1000);
xd= double(subs(xtraj,t,treal));
yd= double(subs(ytraj,t,treal));
xdot= double(subs(xdottmp,t,treal));
ydot= double(subs(ydottmp,t,treal));
%% cal theta value
th1 = zeros(1000);
th2 = zeros(1000);
th1(1) = home1;
th2(1) = home2;
dotmat = zeros(2,1);
for k=1:1:1000
    tmpJ1 = subs(J,[theta1 theta2],[th1(k) th2(k)]);
    Ja = double(tmpJ1(1:2,1:2));
    dotmat(1,1) = xdot(k);
    dotmat(2,1) = ydot(k);
    thdottmp = Ja\dotmat;
    th1(k+1) = th1(k) + thdottmp(1)*0.001;
    th2(k+1) = th2(k) + thdottmp(2)*0.001;
    
end
%% animation
 figure;
h = plot(0,0,0,0);
axis([-0.5 1.5 -1 1]);axis equal;
for k=1:1:1000
   
    Xcoord1 = [0 cosd(th1(k))];
    Ycoord1 = [0, sind(th1(k))];
    Xcoord2 = [cosd(th1(k)), cosd(th1(k))+cosd(th1(k)+th2(k))];
    Ycoord2 = [sind(th1(k)), sind(th1(k))+sind(th1(k)+th2(k))];
    h(1).XData = Xcoord1;
    h(1).YData = Ycoord1;
    h(2).XData = Xcoord2;
    h(2).YData = Ycoord2;
    drawnow;
    if k==1
        pause;
    end
end
function A = DHtheta(theta, d, a, alpha,home)
thetaMat = [cosd(theta+home), -sind(theta+home), 0 , 0;
            sind(theta+home), cosd(theta+home), 0 , 0;
            0           0           1   0;
            0           0           0   1];
dMat = [ 1 0 0 0;
         0 1 0 0;
         0 0 1 d;
         0 0 0 1];
alphaMat = [ 1               0               0           0;
             0       cosd(alpha)      -sind(alpha)        0;
             0       sind(alpha)      cosd(alpha)         0;
             0              0               0           1];
aMat = [ 1 0 0 a;
         0 1 0 0;
         0 0 1 0;
         0 0 0 1];
A = thetaMat*dMat*aMat*alphaMat;
end
function a = amat(q0,q0dot,q02dot,qf,qfdot,qf2dot,t0,tf)
trjmat = [t0^5,     t0^4,   t0^3,  t0^2, t0,  1;
          5*t0^4, 4*t0^3, 3*t0^2,  2*t0,  1,  0;
          20*t0^3, 12*t0^2 6*t0,   2   ,  0,  0 ;
          tf^5,     tf^4,   tf^3,  tf^2, tf,  1;
          5*tf^4, 4*tf^3, 3*tf^2,  2*tf,  1,  0;
          20*tf^3, 12*tf^2 6*tf,   2   ,  0,  0 ];
qmat = [q0;q0dot;q02dot;qf;qfdot;qf2dot];
a = trjmat\qmat;
end