% ------------------------------------- PLOTTING MANIPULABILITY -------------------------------------
link1_pos_=H_m_1_symb(0.1);
link1_pos=(link1_pos_(1:3,4));
link2_pos_=H_m_2_symb(0.1,0.1);
link2_pos=(link2_pos_(1:3,4));
link3_pos_=H_m_3_symb(0.1,0.1,0.1);
link3_pos=(link3_pos_(1:3,4));
link4_pos_=H_m_4_symb(0.1,0.1,0.1,0.1);
link4_pos=(link4_pos_(1:3,4));
link5_pos_=H_m_5_symb(0.1,0.1,0.1,0.1,0.1);
link5_pos=(link5_pos_(1:3,4));
%%
% ------------ 1. TRANSLATIONAL MANIPULABILITY ELLIPSOID -----------------
figure(1001)
mw=5;lw=10;
lw1=0.5;
syms x y z
x_d=[x-link5_pos(1);y-link5_pos(2);z-link5_pos(3)];
% lw=1;
E_V=transpose(x_d)*pinv(p(1:3,:)*transpose(p(1:3,:)))*x_d-1;
fimplicit3(E_V,'-b','LineWidth',lw1,'FaceAlpha',.5) %%%% velocity manipulability Ellipse
hold on
E_VC=transpose(x_d)*pinv(q(1:3,:)*transpose(q(1:3,:)))*x_d-1;
fimplicit3(E_VC,'-r','LineWidth',lw1,'FaceAlpha',.1) %%%% velocity manipulability Ellipse
% 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 Velocity manipulability')
% xlim([-0.5 4]);
% ylim([-2.5 2]);
grid minor;
% axis square ;
legend('Velocity Manipulability Ellipsoid','Combined Velocity Manipulability Ellipsoid','link1', 'link2', 'link3', 'link4', 'link5');
%%
% ------------ 2. ANGULAR MANIPULABILITY ELLIPSOID -----------------
figure(1002)
E_V=transpose(x_d)*pinv(p(4:6,:)*transpose(p(4:6,:)))*x_d-1;
fimplicit3(E_V,'-b','LineWidth',lw1,'FaceAlpha',.5) %%%% velocity manipulability Ellipse
hold on
E_VC=transpose(x_d)*pinv(q(4:6,:)*transpose(q(4:6,:)))*x_d-1;
fimplicit3(E_VC,'-r','LineWidth',lw1,'FaceAlpha',0.1) %%%% velocity manipulability Ellipse
mw=5;lw=10;
lw1=0.5;
% 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 Velocity manipulability')
% xlim([-0.5 4]);
% ylim([-2.5 2]);
grid minor
% axis square ;
legend('Angular Manipulability Ellipsoid','Combined Angular Manipulability Ellipsoid','link1', 'link2', 'link3', 'link4', 'link5');
%%
% ------------ 3. FORCE MANIPULABILITY ELLIPSOID -----------------
syms fx fy fz
f=[fx-link5_pos(1); fy-link5_pos(2); fz-link5_pos(3)];
figure(2001)
E_F=transpose(f)*(p(1:3,:)*p(1:3,:)')*f-1;
fimplicit3(E_F,'-c','LineWidth',lw1,'FaceAlpha',.5) %%%% Force Manipulability Ellipse
hold on
E_FC=transpose(f)*(q(1:3,:)*q(1:3,:)')*f-1;
fimplicit3(E_FC,'-c','LineWidth',lw1,'FaceAlpha',.1) %%%% Force Manipulability Ellipse
mw=5;lw=10;
% 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('Force manipulability')
% xlim([-0.5 4]);
% ylim([-2.5 2]);
grid minor
% axis square ;
legend('Force Manipulability Ellipsoid','Combined Force Manipulability Ellipsoid','link1', 'link2', 'link3', 'link4', 'link5');
%%
% ------------ 4. MOMENT MANIPULABILITY ELLIPSOID -----------------
figure(2002)
lw1=0.5;
E_MC=transpose(f)*(p(4:6,:)*p(4:6,:)')*f-1;
fimplicit3(E_MC,'-c','LineWidth',lw1,'FaceAlpha',.1) %%%% Force Manipulability Ellipse
mw=5;lw=10;
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('Torque manipulability')
% xlim([-0.5 4]);
% ylim([-2.5 2]);
grid minor
% axis square ;
legend('Torque Manipulability Ellipsoid','Combined Torque Manipulability Ellipsoid','link1', 'link2', 'link3', 'link4', 'link5');