%L(i)=Link([theta d a alpha])

L(1)=Link('d',0.495,'a',0.175,'alpha',pi/2);
L(2)=Link('d',0,'a',0.9,'alpha',0,'offset',-pi/2);
L(3)=Link('d',0,'a',0.175,'alpha',-pi/2);
L(4)=Link('d',0.96,'a',0,'alpha',pi/2);
L(5)=Link('d',0,'a',0,'alpha',pi/2,'offset',pi);
L(6)=Link('d',0,'a',0,'alpha',0);
ABB_IRB_4600=SerialLink(L)
ABB_IRB_4600.name='ABB IRB 4600'
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% J= ABB_IRB_4600.jacob0([1.09693068 0.37970577 -0.02032112 1.06759371
1.04877252 2.35900625])
% jsingu(J)
% ABB_IRB_4600.maniplty([1.09693068 0.37970577 -0.02032112 1.06759371 1.04877252
2.359006], 'yoshikawa')
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %linear
% dq=1e-6 %Suppose this very small value
% %Suppose working on joint2
% %Suppose six different angles
% t0= ABB_IRB_4600.fkine([1.09693068 0.37970577 -0.02032112 1.06759371
1.04877252 2.35900625])
% tp1=ABB_IRB_4600.fkine([1.09693068 0.37970577 -0.02032112 1.06759371
1.04877252 2.35900625]+[0 dq 0 0 0 0])
% dTdq1=(tp1-t0)/dq
% speeds=transl(dTdq1)
% x_dot=speeds(1,1) %m/s
% y_dot=speeds(2,1)
% z_dot=speeds(3,1)
% %rotational
% drdq1=dTdq1(1:3,1:3)
% R=t0(1:3,1:3)
% s1=drdq1*R' %R' is R transpose
% vex(s1)
% J=ABB_IRB_4600.jacob0([1.09693068 0.37970577 -0.02032112 1.06759371 1.04877252
2.35900625]+[0 dq 0 0 0 0])
% J0=ABB_IRB_4600.jacob0([0 0 0 0 0 0])
% Ji=ABB_IRB_4600.jacob0([1.09693068 0.37970577 -0.02032112 1.06759371
1.04877252 2.35900625])
% determinant=det(J)
% rank_=rank(J)
% determinant_z=det(J0)
% rank_z=rank(J0)
% determinant_n=det(Ji)
% rank_n=rank(Ji)
% va=[0;0;0.1;0;0;0]
% qd=inv(J)*va %what velocities do joints have at this joint velocity
% vb=[0;0;0;0;0.2;0]
% qd=inv(J)*vb %manupulability goes bad with sigularity
% %% test manupulability
% JJ=ABB_IRB_4600.jacob0([1.09693068 0.37970577 -0.02032112 1.06759371
1.04877252 2.35900625])
% JJ=JJ(1:3,:)
% plot_ellipse(JJ*JJ')%for x_dot, y_dot and z_dot
% figure
% %Suppose another six different angles
% JJ=ABB_IRB_4600.jacob0([0.09693068 2.37970577 -1.02032112 2.06759371
0.04877252 3.35900625])
% JJ=JJ(4:6,:)
% plot_ellipse(JJ*JJ')
% Jacobian, velocity, manipulability, singularity