% ------------------------------------- STIFFNESS ANALYSIS -------------------------------------
syms theta1 theta2 theta3 theta4 theta5
J_manipulator=J_manipulator_symb(0.1,0.1,0.1,0.1,0.1)
%%
% ------------ 1. JOINT STIFFNESS ANALYSIS: Kq -----------------
%%% Lets assume a symmetric and cosntant joint stiffness matrix as
vv=[100 100 100 100 100]; %%% defined diagonal just to take decoupled kq
kq=diag(vv);
%%
% ------------ 2. GEOMETRIC STIFFNESS ANALYSIS: Kg -----------------
a=1;
Fext=[a; a; a; 0; 0; 0];
% syms theta1 theta2 theta3 theta4 theta5 real
% t=[theta1; theta2; theta3; theta4; theta5];
% kg=zeros(5,5);
kg1=(diff(transpose(J_manipulator),theta1))*Fext;
kg2=(diff(transpose(J_manipulator),theta2))*Fext;
kg3=(diff(transpose(J_manipulator),theta3))*Fext;
kg4=(diff(transpose(J_manipulator),theta4))*Fext;
kg5=(diff(transpose(J_manipulator),theta5))*Fext;
kg=[kg1 kg2 kg3 kg4 kg5];
%%
% singularValues=sqrt(J_combined*transpose(J_combined));
% singularValues1=simplify(rref(J_combined))
%%
% ------------ 3. TASKSPACE STIFFNESS ANALYSIS: Kx -----------------
Kx=vpa(pinv(transpose(J_manipulator))*(kq-kg)*pinv(J_manipulator),3)
eig(Kx(4:6,:)*Kx(4:6,:)')
%%
% ------------------------------------- TRANSLATIONAL TASKSPACE STIFFNESS ELLIPSOID -------------------------------------
figure(3001)
K_X=transpose(x_d)*pinv(Kx(1:3,:)*transpose(Kx(1:3,:)))*x_d-1;
fimplicit3(E_V,'-r','LineWidth',lw1,'FaceAlpha',.5) %%%% velocity manipulability Ellipse
mw=5;lw=10;
lw1=0.5;
hold on
% plot3(0,0,0,'og','LineWidth',mw) %%% ground marker
plot3([0 link1_pos(1)], [0 link1_pos(2)],[0 link1_pos(3)],'-c','LineWidth',lw) %%% link1
plot3([link1_pos(1) link2_pos(1)], [link1_pos(2) link2_pos(2)],[link1_pos(3) link2_pos(3)],'-r','LineWidth',lw) %%% link2
plot3([link2_pos(1) link3_pos(1)],[link2_pos(2) link3_pos(2)],[link2_pos(3) link3_pos(3)],'-g','LineWidth',lw) %%%% link3
plot3([link3_pos(1) link4_pos(1)],[link3_pos(2) link4_pos(2)],[link3_pos(3) link4_pos(3)],'-b','LineWidth',lw) %%%% link4
plot3([link4_pos(1) link5_pos(1)],[link4_pos(2) link5_pos(2)],[link4_pos(3) link5_pos(3)],'-k','LineWidth',lw) %%%% link5
xlabel('x');
ylabel('y');
zlabel('z');
title('Linear Stiffness Ellipsoid')
% xlim([-0.5 4]);
% ylim([-2.5 2]);
grid minor
% axis square ;
legend('Linear taskspace stiffness Ellipsoid','link1','link2','link3','link4','link5')
%%
% ------------------------------------- ANGULAR TASKSPACE STIFFNESS ELLIPSOID -------------------------------------
figure(3002)
K_T=transpose(x_d)*pinv(Kx(4:6,:)*transpose(Kx(4:6,:)))*x_d-1;
fimplicit3(E_V,'-r','LineWidth',lw1,'FaceAlpha',.5) %%%% velocity manipulability Ellipse
mw=5;lw=10;
lw1=0.5;
hold on
% plot3(0,0,0,'og','LineWidth',mw) %%% ground marker
plot3([0 link1_pos(1)], [0 link1_pos(2)],[0 link1_pos(3)],'-c','LineWidth',lw) %%% link1
plot3([link1_pos(1) link2_pos(1)], [link1_pos(2) link2_pos(2)],[link1_pos(3) link2_pos(3)],'-r','LineWidth',lw) %%% link2
plot3([link2_pos(1) link3_pos(1)],[link2_pos(2) link3_pos(2)],[link2_pos(3) link3_pos(3)],'-g','LineWidth',lw) %%%% link3
plot3([link3_pos(1) link4_pos(1)],[link3_pos(2) link4_pos(2)],[link3_pos(3) link4_pos(3)],'-b','LineWidth',lw) %%%% link4
plot3([link4_pos(1) link5_pos(1)],[link4_pos(2) link5_pos(2)],[link4_pos(3) link5_pos(3)],'-k','LineWidth',lw) %%%% link5
xlabel('x');
ylabel('y');
zlabel('z');
title('Angular Stiffness Ellipsoid')
% xlim([-0.5 4]);
% ylim([-2.5 2]);
grid minor
% axis square ;
legend('Rotational taskspace stiffness Ellipsoid','link1','link2','link3','link4','link5')