function [qdot]=Mydiff(t,x)
global lengths radiusx radiusy ell_angle
ell_an=ell_angle*pi/180;
A1=[ lengths(2,1)*sin(x(4))+lengths(2,2)*sin(x(5)+x(4)), lengths(2,2)*sin(x(5)+x(4)), lengths(2,3)*sin(x(6)), 0, 0, 0, 0, 0, 0;
-lengths(2,1)*cos(x(4))-lengths(2,2)*cos(x(5)+x(4)), -lengths(2,2)*cos(x(5)+x(4)), -lengths(2,3)*cos(x(6)), 0, 0, 0, 0, 0, 0;
0, 0, 1, 0, 0, 0, 0, 0, 0;
0, 0, 0, lengths(3,1)*sin(x(7))+lengths(3,2)*sin(x(8)+x(7)), lengths(3,2)*sin(x(8)+x(7)), lengths(3,3)*sin(x(9)), 0, 0, 0;
0, 0, 0, -lengths(3,1)*cos(x(7))-lengths(3,2)*cos(x(8)+x(7)), -lengths(3,2)*cos(x(8)+x(7)), -lengths(3,3)*cos(x(9)), 0, 0, 0;
0, 0, 0, 0, 0, 1, 0, 0, 0;
0, 0, 0, 0, 0, 0, lengths(4,1)*sin(x(10))+lengths(4,2)*sin(x(11)+x(10)), lengths(4,2)*sin(x(11)+x(10)), lengths(4,3)*sin(x(12));
0, 0, 0, 0, 0, 0, -lengths(4,1)*cos(x(10))-lengths(4,2)*cos(x(11)+x(10)), -lengths(4,2)*cos(x(11)+x(10)), -lengths(4,3)*cos(x(12));
0, 0, 0, 0, 0, 0, 0, 0, 1];
B=[-lengths(1,1)*sin(x(1))-lengths(1,2)*sin(x(2)+x(1)), -lengths(1,2)*sin(x(2)+x(1)), -lengths(1,3)*sin(x(3));
lengths(1,1)*cos(x(1))+lengths(1,2)*cos(x(2)+x(1)), lengths(1,2)*cos(x(2)+x(1)), lengths(1,3)*cos(x(3));
0, 0, 1;];
B1=[B;B;B];
% X=radiusx*cos(t);
% Y=radiusy*sin(t);
xdot=-radiusx*sin(t)*cos(ell_an)-radiusy*cos(t)*sin(ell_an);
ydot=-radiusx*sin(t)*sin(ell_an)+radiusy*cos(t)*cos(ell_an);
C=-pinv(A1)*B1;
D=[eye(3); C];
J = [-lengths(1,1)*sin(x(1))-lengths(1,2)*sin(x(1)+x(2)), -lengths(1,2)*sin(x(2)+x(1)), -lengths(1,3)*sin(x(3));
lengths(1,1)*cos(x(1))+lengths(1,2)*cos(x(1)+x(2)), lengths(2,1)*cos(x(2)+x(1)), lengths(1,3)*cos(x(3));
0, 0, 1];
J1=J*D(1:3,:);
Ji=pinv(J1);
NullSpace=eye(3)-Ji*J1;
qidot=Ji*[xdot;ydot;0]+NullSpace*[pi/2-x(1);pi-x(2);0-x(3)];
qdot=D*qidot;
% pause
