% 
% Design of personalized wearable haptic interfaces to account for fingertip size and shape
%
% Monica Malvezzi, Francesco Chinello, Domenico Prattichizzo, Claudio
% Pacchierotti
%
%    thimble3RRS_design.m - design adaptation of a 3RRS parallel mechanism for
%    fingertip cutaneous feedback to user's specifics and requirements
%    
%
%    Usage: thimbleRRS.m
%
% 
%  Copyright (c) 2020,  M. Malvezzi, F. Chinello, D. Prattichizzo, C.
% Pacchierotti --- Version: 1p1
%
%  All rights reserved.
% 
%  Redistribution and use with or without
%  modification, are permitted provided that the following conditions are met:
%      * Redistributions of source code must retain the above copyright
%        notice, this list of conditions and the following disclaimer.
%      * Redistributions in binary form must reproduce the above copyright
%        notice, this list of conditions and the following disclaimer in the
%        documentation and/or other materials provided with the distribution.
%      * Neither the name of the <organization> nor the
%        names of its contributors may be used to endorse or promote products
%        derived from this software without specific prior written permission.
% 
%  THIS SOFTWARE IS PROVIDED BY  M. Malvezzi, F. Chinello, D. Prattichizzo, C.
%  Pacchierotti ``AS IS'' AND ANY
%  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
%  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
%  DISCLAIMED. IN NO EVENT SHALL M. Malvezzi, F. Chinello, D. Prattichizzo, C.
% Pacchierotti BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
%  EXEMPLARY, OR CONSEQUENTIAL DAMAGES
%  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
%  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
%  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
%  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
%  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

%% start
clear variables
close all
clc

% Define the main parameters of 
% platform dimension constraints

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%  INPUT DATA BELOW %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Display some info for the user
info = 0;
if(info)
    disp('-----------------------------------------')
    disp(' ')
    disp('Design of personalized wearable haptic interfaces to account for fingertip size and shape')
    disp(' ')
    disp('Monica Malvezzi, Francesco Chinello, Domenico Prattichizzo, Claudio Pacchierotti')
    disp(' ')
    disp('-----------------------------------------')
    %
end
% STEP 1 measure e1, e2, e3 and fix psimax, thetamax, zetamax
disp(' ')
disp('-----------------------------------------')
disp(' ')
disp('STEP 1 measure e1, e2, e3 and fix psimax, thetamax, zetamax ')
disp(' ')
disp('STEP 2 insert these data in the procedure')
disp(' ')
in1 = input('Finger width, 2*e1 [mm]: ');
in2 = input('Finger thickness, 2*e2 [mm]: ');
in3 = input('Finger longitudinal radius, e3 [mm]: ');
% HARDWARE/PERFORMANCE DATA
psimaxdeg = 30;    % deg
thetamaxdeg = 30;  % deg
zetamax = 5;       % mm
disp(' ')
disp('psimax, thetamax, zetamax are fixed as follows ')
disp(['psimax = ',num2str(psimaxdeg),' deg'])
disp(['thetamax = ',num2str(thetamaxdeg),' deg'])
disp(['zetamax = ',num2str(thetamaxdeg),' deg'])
disp(' ')
disp('to change these values, check the script ')
disp(' ')
input('Press any key to continue');
% The following lines can be uncommented in case maximum values are different from default
%
%psimaxdeg = input('maximum roll angle  [deg]');
%thetamaxdeg = input('maximum pitch angle  [deg]');
%zetamax = input('maximum displacement in z direction [mm]');

% convert to m
in1 = in1/1000;
in2 = in2/1000;
in3 = in3/1000;

zetamax=zetamax/1000; 

% Other HARDWARE CONSTRAINTS
% fixed distance in z direction between the finger top surface and the motors 
Hplat = 8;
%  uncomment in case a different Hplat is necessary
%
%Hplat = input('Fixed offset between finger top surface and device motors [mm] ');
Hplat=Hplat/1000; % transforms it into meters

% these are  minimum values for device dimension that could be set to 
% take into account manufacturing and hardware limits
aminfixed = 0.00; % 15.00;
bminfixed = 0.00; % 10.00;

aminfixed = aminfixed/1000;
bminfixed = bminfixed/1000;
l1min = 8; % minimum length of device leg from hardware constraints
l2min = 8; % minimum length of device leg from hardware constraints

%  uncomment in case a different Hplat is necessary
%
%l1min = input('Minimum length of l1 link [mm] ');
%l2min = input('Minimum length of l2 link [mm] ');

l1min = l1min/1000; % convert to m
l2min = l2min/1000; % convert to m

% we assume to fix the platform aspect ratio 
ratioplat = sqrt(3); % ad/a = bd/b

%% plot of the fingertip approximated surface
% the tip is approximated as an ellipsoid

e1 = in2/2;
e2 = in1/2;
e3 = in3;

 psimax = psimaxdeg*pi/180;
 thetamax = thetamaxdeg*pi/180;

disp('-----------------------------------------')
disp(' ')
disp('STEP3 evaluate the best a, b, ad, bd, l1 and l2')
disp(' ')

% STEP 3 evaluate the best a, b, ad, bd, l1 and l2

% STEP 3.1 evaluation of mobile platform parameters
[xe,ye,ze] = ellipsoid(0,0,0,e1,e2,e3,50);

% rotation is necessary to align the ellipsoid to the reference frame
% x - longitudinal
% y - lateral
% z - normal 
 Roty = [0 0 -1;
     0 1 0
     1 0 0];
 hold on
 % plot the fingertip
 figure(1)
 axis('equal')

 xerot = zeros(size(ze));
 yerot = zeros(size(ze));
 zerot = zeros(size(ze));
 for i =1:size(ze,1)
     for j = 1:size(ze,2)
         vettmp= [xe(i,j), ye(i,j), ze(i,j)]';
         vettmprot = Roty*vettmp;
         xerot(i,j) = vettmprot(1);
         yerot(i,j) = vettmprot(2);
         zerot(i,j) = vettmprot(3);
         
     end
 end
 s=surf(xerot(1:26,:),yerot(1:26,:),zerot(1:26,:))
 axis('equal')
 xlabel('x (m)')
 ylabel('y (m)')
 zlabel('z (m)')
 colormap('pink');
s.EdgeColor = 'none';
%title('Finger tip ellipsoid approximation and contact patch (red)')
% 

% Defining the contact area 
% psi roll angle - range 
% this value describes the discretization of roll angle
deltapsi =0.02;
psi = -psimax:deltapsi:psimax;
% theta pitch angle - range
% this value describes the discretization of roll angle
deltatheta = 0.02;
theta = 0:deltatheta:thetamax;
theta=theta-pi/2;

% defining points of C set
hold on
zc = zeros(length(psi),length(theta));
yc = zeros(length(psi),length(theta));
xc = zeros(length(psi),length(theta));
for i=1:length(psi)
    for j = 1:length(theta)
        zc(i,j) = e1*cos(psi(i))*sin(theta(j));
        yc(i,j) = e2*sin(psi(i))*sin(theta(j));
        xc(i,j) = e3*cos(theta(j));
    end
end
plot3(xc,yc,zc,'co')

psiaus = [0:deltapsi:psimax];
    xcr(1,1)= 0;

% evaluate the rectified surface for designing the mobile platform    
for j = 2:length(theta)
    xcr(1,j)= xcr(1,j-1)+sqrt(e3^2*sin(theta(j))^2+e1^2*cos(theta(j))^2)*deltatheta;
    ycr(1,j)= 0;
    zcr(1,j)= -1.5*e1;
    %plot3(xcr(1,j),ycr(1,j),zcr(1,j),'b*')

    for i=2:length(psiaus)
        e1r = e1*sin(theta(j));
        e2r = e2*sin(theta(j));
        zcr(i,j) = -1.5*e1; % not necessary, just for visualization
        ycr(i,j) = ycr(i-1,j)+sqrt(e2r^2*sin(psiaus(i))^2+e1r^2*cos(psiaus(i))^2)*deltapsi;
        xcr(i,j) =   xcr(1,j);
        
    end
end

xcrtot = [xcr;xcr];
ycrtot = [ycr;-ycr];
zcrtot = [zcr,zcr];
k = convhull(xcrtot,ycrtot);
xprofile = xcrtot(k);
yprofile = ycrtot(k);

% plot the contact patch and the mobile platform
figure(2)
%plot(xcrtot,ycrtot,'b*',xprofile,yprofile,'r-')
fill(xprofile,yprofile,'c')
axis('equal')
hold on
title('Contact patch on plane xy (filled area) and mobile platform ')
xlabel('x (m)')
ylabel('y (m)')

bdmin =  2*(max(yprofile)+max(xprofile)/(ratioplat/2)/2);
bmin = bdmin/ratioplat;
bhmin = sqrt((bmin)^2-(bdmin/2)^2);

xb1min = bmin+bhmin;

plot([xb1min,0,0,xb1min],[0,bdmin/2,-bdmin/2,0],'ko-')
fill(xprofile,yprofile,'c')
axis([-0.002 0.03 -0.015 0.015])
legend('contact patch, rectified','mobile platform' )

% output: mobile platform dimension
b = max(bmin,bminfixed);
bd = b* ratioplat;
bh = sqrt(b^2 - (bd/2)^2);

% STEP 3.2 evaluation of fixed platform parameters
ad_min = e2+(bd-e2)*(e1+Hplat)/e1;
amin = ad_min/ratioplat;

% output: fixed platform dimension
a = max(amin,aminfixed);
ad = a* ratioplat;
ah = sqrt(a^2 - (ad/2)^2);


% platform vertex
% fixed platform in the fixed frame
a1 = [a 0 0]';
a2 = [-ah -ad/2 0]';
a3 = [-ah ad/2 0]';

% mobile platform in the mobile frame
b1_1 = [b 0 0]';
b2_1 = [-bh -bd/2 0]';
b3_1 = [-bh bd/2 0]';

% STEP 3.3 defining leg dimensions avoiding device singular configurations
figure(3)
% STEP 3.3.1 defining the set of feasible l1 and l2 value
 
for i = 1:length(psi)
    for j = 1:length(theta)
        %         %pz = pz0*(2-cos(psi(i)))/2;
        pz(i,j) = e1 + Hplat + e1/cos(psi(i));
        % % from these values, given the parallel mechanism constraints, evaluate the
        % % x and y displacements and the phi yaw angle
        % % calculate the phi yaw angle
        phi(i,j) = atan((b-bh)/bh*sin(theta(j))*sin(psi(i))/(cos(theta(j))+cos(psi(i))));
        % % calculate y displacement
        py(i,j) = -b*sin(phi(i,j))*cos(theta(j));
        % % calculate x displacement
        px(i,j) =bh*(cos(phi(i,j))*cos(theta(j))-sin(phi(i,j))*sin(theta(j))*sin(psi(i))...
            -cos(phi(i,j))*cos(psi(i)));
        %  mobile platform complete displacement
        p = [px(i,j) py(i,j) pz(i,j)]';
        Rroll = [1 0 0;
            0 cos(psi(i)) -sin(psi(i));
            0 sin(psi(i)) cos(psi(i))];
        Rpitch = [cos(theta(j)) 0 sin(theta(j));
            0 1 0;
            -sin(theta(j)) 0 cos(theta(j))];
        Ryaw = [cos(phi(i,j)) -sin(phi(i,j)) 0;
            sin(phi(i,j)) cos(phi(i,j)) 0;
            0 0 1];
        R = Ryaw*Rpitch*Rroll;
        %
        % mobile platform vertex coordinates in the fixed ref frame
        %
        b1  = p + R*b1_1;
        b2  = p + R*b2_1;
        b3  = p + R*b3_1;
        % %
        % evaluate leg unit vectors
        s1 = (b1-a1)/norm(b1-a1);
        s2 = (b2-a2)/norm(b2-a2);
        s3 = (b3-a3)/norm(b3-a3);
        %
        % evaluate fixed platform unit vectors
        q1 = a1/norm(a1);
        q2 = a2/norm(a2);
        q3 = a3/norm(a3);
        % %
        % % % evaluate leg angles
        beta1 = pi-acos(s1'*q1);
        beta2 = pi-acos(s2'*q2);
        beta3 = pi-acos(s3'*q3);
        % %
        % evaluate leg length
        d1 = norm(b1-a1);
        d2 = norm(b2-a2);
        d3 = norm(b3-a3);
        % store values
        d1m(i,j) = d1;
        d2m(i,j) = d2;
        d3m(i,j) = d3;
        
    end
end
% % 
dmin = min([min(min(d1m)) min(min(d2m)) min(min(d3m))]);
% %  
dmax = max([max(max(d1m)) max(max(d2m)) max(max(d3m))]);
% %  
% % 
alphamin = 0.01*pi;
alphamax = 0.99*pi;
% % 
l1v = [0:0.0002:0.04];
l2v = [0:0.0002:0.04];
% % % 
conmin1 = l1v*cos(alphamin)+ sqrt(l1v.^2*(cos(alphamin)^2-1)+dmin^2);
conmin2 = l1v*cos(alphamin)- sqrt(l1v.^2*(cos(alphamin)^2-1)+dmin^2);
% %  
conmax =  l1v*cos(alphamax)+ sqrt(l1v.^2*(cos(alphamax)^2-1)+dmax^2);
 %  
 % 
 % 
h = plot(l1v,conmin1,'r');
hold on
set(h,'LineWidth',3)
h = plot(l1v,conmax,'b');
set(h,'LineWidth',3)

hold on
hmin1 = area(l1v,conmin1);
set(hmin1,'FaceColor',[0.1,0.9,0.9],'LineWidth',0.01)
hmin2 = area(l1v,conmin2);
h = plot(l1v,conmin2,'r');
set(h(1),'LineWidth',3)
set(hmin2,'FaceColor',[1,1,1],'LineWidth',0.01)
hmin3 = area(l1v,conmax);
set(hmin3,'FaceColor',[1,1,1],'LineWidth',0.01)
h = plot(l1v,conmax,'b');
set(h(1),'LineWidth',3)
axis([0,0.04,0,0.04])


figure(3)
title('Filled area: l_1 and l_2 values allowing to avoid kinematics singularities  ')
xlabel('l_1  (m)')
ylabel('l_2  (m)')

% STEP 3.3.2 choose l1 and l2 automatically or manually by the user

disp('---------')
disp('How are l1 and l2 defined? ')
disp('1- Automatically, avoiding kinematics singularities and with a lower bound for l1 ')
disp('2- Manually selected by the user by picking a point on Figure 3')
disp(' ')
reply = input('Select an option (1/2 [1])','s');
if ischar(reply)== 0 || length(reply)~=1 || (reply ~= '1' && reply ~= '2')
    disp('Default option selected')
    reply = '1';
end

input('Press any key to continue');

if reply == '1'
    % automatically find the l1 l2 solution: pick l1 and l2 so that all the
    % constraints are satisfied and l1 is as small as possible
    [tmp,isol] = min(abs(conmin1 - conmax));
    l1tmp = l1v(isol);
    l2tmp = conmax(isol);
    % check hardware constraints
    if l1tmp<l1min
        [tmp,isolup] = min(abs(l1v-l1min));
        l1 = l1v(isolup);
        l2 = conmax(isolup);
    else
        l1 = l1tmp;
        l2 = l2tmp;
    end
else
    % the user manually selects l1 and l2 values by picking a value on the
    % diagram
    [l1,l2]= ginput(1);
    [tmp,checkcon1] = min(abs(l1-l1v));
    % a warning appears if the selected couple does not satisfy the
    % constraints
    if l2>conmin1(checkcon1) || l2<conmin2(checkcon1) || l2<conmax(checkcon1)
        disp('Warning - the selected configuration does not guarantee against singular configurations')
    elseif l1<l1min
        disp(['Warning - l_1 value is lower than the minimum constraint l_1min = ',num2str(l1min*1000),' mm'])
    end
end
% plot the selected l1,l2 couple on the diagram
plot(l1,l2,'r*','MarkerSize',10)

% collect the data for STEP 4 
a_mm = a * 1000;
b_mm = b * 1000;
ad_mm = ad * 1000;
bd_mm =bd * 1000;
l1_mm = l1 * 1000;
l2_mm = l2 * 1000;
%
clc 
disp('-------------------');
disp(' ');
disp('STEP 4 insert the personalized dimensions into OnShape');
disp(' ');
disp(['a = ',num2str(a_mm),' mm']);
disp(['b = ',num2str(b_mm),' mm']);
disp(['ad = ',num2str(ad_mm),' mm']);
disp(['bd = ',num2str(bd_mm),' mm']);
disp(['l1 = ',num2str(l1_mm),' mm']);
disp(['l2 = ',num2str(l2_mm),' mm']);
disp(' ');
disp('-------------------');
% ---------------------------------------------------
% Step 5 - device analysis in a specific configuration
%
% for this part of the script, please refer to
% F. Chinello, C. Pacchierotti, M. Malvezzi, D. Prattichizzo. 
% A Three Revolute-Revolute-Spherical wearable fingertip 
% cutaneous device for stiffness rendering. IEEE Transactions 
% on Haptics, 11(1):39-50, 2018.
%
% NOTE: in the following the pure kinematics is evaluated, the contact
% with the finger is not considered
% Device configuration - change these values to modify platform
% configuration
% z displacement of the mobile platform
% - this is the configuration in which the device touches the finger in the
% reference configuration - change it to test other configurations
pz = Hplat+2*e1;
 % psi roll angle - default configuration, change these values if you want
 % to simulate a different configuration.
psi = 0.;
 % theta pitch angle
 theta = 0.;
% 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % platform vertex
 % fixed platform in the fixed frame
 a1 = [a 0 0]';
 a2 = [-ah -ad/2 0]';
 a3 = [-ah ad/2 0]';
% 
 % mobile platform in the mobile frame
 b1_1 = [b 0 0]';
 b2_1 = [-bh -bd/2 0]';
 b3_1 = [-bh bd/2 0]';
% 
 % Complete platform configuration        
 % Given the parallel mechanism constraints, evaluate the
 % x and y displacements and the phi yaw angle
 % calculate the phi yaw angle
 phi = atan((b-bh)/bh*sin(theta)*sin(psi)/(cos(theta)+cos(psi)));
% % calculate y displacement
 py = -b*sin(phi)*cos(theta);
% % calculate x displacement
 px =bh*(cos(phi)*cos(theta)-sin(phi)*sin(theta)*sin(psi)-cos(phi)*cos(psi));
% 
 % mobile platform complete displacement
 p = [px py pz]';
 Rroll = [1 0 0;
     0 cos(psi) -sin(psi);
     0 sin(psi) cos(psi)];
 Rpitch = [cos(theta) 0 sin(theta);
     0 1 0;
     -sin(theta) 0 cos(theta)];
 Ryaw = [cos(phi) -sin(phi) 0;
     sin(phi) cos(phi) 0;
     0 0 1];
 R = Ryaw*Rpitch*Rroll;
%         
 % mobile platform vertex coordinates in the fixed ref frame
% 
 b1  = p + R*b1_1;
 b2  = p + R*b2_1;
 b3  = p + R*b3_1;
% 
% 
% 
% evaluate leg unit vectors
 s1 = (b1-a1)/norm(b1-a1);
 s2 = (b2-a2)/norm(b2-a2);
 s3 = (b3-a3)/norm(b3-a3);
% 
 % evaluate fixed platform unit vectors
 q1 = a1/norm(a1);
 q2 = a2/norm(a2);
 q3 = a3/norm(a3);
% 
 % evaluate leg angles
 beta1 = pi-acos(s1'*q1);
 beta2 = pi-acos(s2'*q2);
 beta3 = pi-acos(s3'*q3);
% 
 % evaluate leg length
 d1 = norm(b1-a1);
 d2 = norm(b2-a2);
 d3 = norm(b3-a3);
% 
 % evaluate leg  angles
 gamma1 = acos((l1^2+d1^2-l2^2)/(2*l1*d1));
 gamma2 = acos((l1^2+d2^2-l2^2)/(2*l1*d2));
 gamma3 = acos((l1^2+d3^2-l2^2)/(2*l1*d3));
% 
 % evaluate leg base angle
 alpha1 = atan((b1(3)-a1(3))/sqrt((b1(1)-a1(1))^2+(b1(2)-a1(2))^2));
 alpha2 = atan((b2(3)-a2(3))/sqrt((b2(1)-a2(1))^2+(b2(2)-a2(2))^2));
 alpha3 = atan((b3(3)-a3(3))/sqrt((b3(1)-a3(1))^2+(b3(2)-a3(2))^2));
% 
 % evaluate actuator rotation angles
 % this is the output of the inverse kinematics of the mechanism
 delta1 = pi-alpha1-gamma1;
 delta2 = pi-alpha2-gamma2;
 delta3 = pi-alpha3-gamma3;
%         
 % evaluate knee position coordinates in the local vertex frames
 k1_1 = [a+l1*cos(delta1) 0 l1*sin(delta1)]';
 k2_2 = [a+l1*cos(delta2) 0 l1*sin(delta2)]';
 k3_3 = [a+l1*cos(delta3) 0 l1*sin(delta3)]';
% 
 % local vertex frame rotations
 chi1 = atan2(a1(2),a1(1));
 chi2 = atan2(a2(2),a2(1));
 chi3 = atan2(a3(2),a3(1));
 Rchi1 = [cos(chi1) -sin(chi1) 0;
     sin(chi1) cos(chi1) 0;
     0 0 1];
 Rchi2 = [cos(chi2) -sin(chi2) 0;
     sin(chi2) cos(chi2) 0;
     0 0 1];
 Rchi3 = [cos(chi3) -sin(chi3) 0;
     sin(chi3) cos(chi3) 0;
     0 0 1];
 
 % knee points in the global frame
 k1 = Rchi1*k1_1;
 k2 = Rchi2*k2_2;
 k3 = Rchi3*k3_3;
% 
 % joint unit vectors
 kvers = [0 0 1]';
 a1vers = [cos(chi1) sin(chi1) 0]';
 a2vers = [cos(chi2) sin(chi2) 0]';
 a3vers = [cos(chi3) sin(chi3) 0]';
% 
 u1vers = cross(kvers,a1vers);
 u2vers = cross(kvers,a2vers);
 u3vers = cross(kvers,a3vers);
% 

 % plot - device scheme -------
figure(1) % in this case plot the device with the finger, change the number of
% figure to create a new image
axis('equal')% 
% Design of personalized wearable haptic interfaces to account for fingertip size and shape
%
% Monica Malvezzi, Francesco Chinello, Domenico Prattichizzo, 
% Claudio Pacchierotti
%
%    thimble3RRS_design.m - design adaptation of a 3RRS parallel mechanism for
%    fingertip cutaneous feedback to user's specifics and requirements
%    
%
%    Usage: thimbleRRS.m
%
% 
%  Copyright (c) 2020,  M. Malvezzi, F. Chinello, D. Prattichizzo, C.
% Pacchierotti --- Version: 1p1
%
%  All rights reserved.
% 
%  Redistribution and use with or without
%  modification, are permitted provided that the following conditions are met:
%      * Redistributions of source code must retain the above copyright
%        notice, this list of conditions and the following disclaimer.
%      * Redistributions in binary form must reproduce the above copyright
%        notice, this list of conditions and the following disclaimer in the
%        documentation and/or other materials provided with the distribution.
%      * Neither the name of the <organization> nor the
%        names of its contributors may be used to endorse or promote products
%        derived from this software without specific prior written permission.
% 
%  THIS SOFTWARE IS PROVIDED BY  M. Malvezzi, F. Chinello, D. Prattichizzo, C.
%  Pacchierotti ``AS IS'' AND ANY
%  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
%  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
%  DISCLAIMED. IN NO EVENT SHALL M. Malvezzi, F. Chinello, D. Prattichizzo, C.
% Pacchierotti BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
%  EXEMPLARY, OR CONSEQUENTIAL DAMAGES
%  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
%  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
%  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
%  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
%  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

%% start
clear variables
close all
clc

% Define the main parameters of 
% platform dimension constraints

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%  INPUT DATA BELOW %%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Display some info for the user
info = 0;
if(info)
    disp('-----------------------------------------')
    disp(' ')
    disp('Design of personalized wearable haptic interfaces to account for fingertip size and shape')
    disp(' ')
    disp('Monica Malvezzi, Francesco Chinello, Domenico Prattichizzo, Claudio Pacchierotti')
    disp(' ')
    disp('-----------------------------------------')
    %
end
% STEP 1 measure e1, e2, e3 and fix psimax, thetamax, zetamax
disp(' ')
disp('-----------------------------------------')
disp(' ')
disp('STEP 1 measure e1, e2, e3 and fix psimax, thetamax, zetamax ')
disp(' ')
disp('STEP 2 insert these data in the procedure')
disp(' ')
in1 = input('Finger width, 2*e1 [mm]: ');
in2 = input('Finger thickness, 2*e2 [mm]: ');
in3 = input('Finger longitudinal radius, e3 [mm]: ');
% HARDWARE/PERFORMANCE DATA
psimaxdeg = 30;    % deg
thetamaxdeg = 30;  % deg
%zetamax = 5;       % mm
zetamax = 10;
disp(' ')
disp('psimax, thetamax, zetamax are fixed as follows ')
disp(['psimax = ',num2str(psimaxdeg),' deg'])
disp(['thetamax = ',num2str(thetamaxdeg),' deg'])
%disp(['zetamax = ',num2str(thetamaxdeg),' deg']) 
disp(['zetamax = ',num2str(zetamax),' mm'])
disp(' ')
disp('to change these values, check the script ')
disp(' ')
input('Press any key to continue');
% The following lines can be uncommented in case maximum values are different from default
%
%psimaxdeg = input('maximum roll angle  [deg]');
%thetamaxdeg = input('maximum pitch angle  [deg]');
%zetamax = input('maximum displacement in z direction [mm]');

% convert to m
in1 = in1/1000;
in2 = in2/1000;
in3 = in3/1000;

zetamax=zetamax/1000; 

% Other HARDWARE CONSTRAINTS
% fixed distance in z direction between the finger top surface and the motors 
Hplat = 8;
%  uncomment in case a different Hplat is necessary
%
%Hplat = input('Fixed offset between finger top surface and device motors [mm] ');
Hplat=Hplat/1000; % transforms it into meters

% these are  minimum values for device dimension that could be set to 
% take into account manufacturing and hardware limits
aminfixed = 0.00; % 15.00;
bminfixed = 10.00; % 10.00;

aminfixed = aminfixed/1000;
bminfixed = bminfixed/1000;
l1min = 8; % minimum length of device leg from hardware constraints
l2min = 8; % minimum length of device leg from hardware constraints

%  uncomment in case a different Hplat is necessary
%
%l1min = input('Minimum length of l1 link [mm] ');
%l2min = input('Minimum length of l2 link [mm] ');

l1min = l1min/1000; % convert to m
l2min = l2min/1000; % convert to m

% we assume to fix the platform aspect ratio 
ratioplat = sqrt(3); % ad/a = bd/b

%% plot of the fingertip approximated surface
% the tip is approximated as an ellipsoid

e1 = in1/2;
e2 = in2/2;
e3 = in3;

 psimax = psimaxdeg*pi/180;
 thetamax = thetamaxdeg*pi/180;

disp('-----------------------------------------')
disp(' ')
disp('STEP3 evaluate the best a, b, ad, bd, l1 and l2')
disp(' ')

% STEP 3 evaluate the best a, b, ad, bd, l1 and l2

% STEP 3.1 evaluation of mobile platform parameters
[xe,ye,ze] = ellipsoid(0,0,0,e1,e2,e3,50);

% rotation is necessary to align the ellipsoid to the reference frame
% x - longitudinal
% y - lateral
% z - normal 
 Roty = [0 0 -1;
     0 1 0
     1 0 0];
 hold on
 % plot the fingertip
 figure(1)
 axis('equal')

 xerot = zeros(size(ze));
 yerot = zeros(size(ze));
 zerot = zeros(size(ze));
 for i =1:size(ze,1)
     for j = 1:size(ze,2)
         vettmp= [xe(i,j), ye(i,j), ze(i,j)]';
         vettmprot = Roty*vettmp;
         xerot(i,j) = vettmprot(1);
         yerot(i,j) = vettmprot(2);
         zerot(i,j) = vettmprot(3);
         
     end
 end
 s=surf(xerot(1:26,:),yerot(1:26,:),zerot(1:26,:))
 axis('equal')
 xlabel('x (m)')
 ylabel('y (m)')
 zlabel('z (m)')
 colormap('pink');
s.EdgeColor = 'none';
%title('Finger tip ellipsoid approximation and contact patch (red)')
% 

% Defining the contact area 
% psi roll angle - range 
% this value describes the discretization of roll angle
deltapsi =0.02;
psi = -psimax:deltapsi:psimax;
% theta pitch angle - range
% this value describes the discretization of roll angle
deltatheta = 0.02;
theta = 0:deltatheta:thetamax;
theta=theta-pi/2;

% defining points of C set
hold on
zc = zeros(length(psi),length(theta));
yc = zeros(length(psi),length(theta));
xc = zeros(length(psi),length(theta));
for i=1:length(psi)
    for j = 1:length(theta)
        zc(i,j) = e1 * cos(psi(i))*sin(theta(j));
        yc(i,j) = e2 * sin(psi(i))*sin(theta(j));
        xc(i,j) = e3 * cos(theta(j));
    end
end
plot3(xc,yc,zc,'co')

psiaus = [0:deltapsi:psimax];
    xcr(1,1)= 0;

% evaluate the rectified surface for designing the mobile platform    
for j = 2:length(theta)
    xcr(1,j)= xcr(1,j-1)+sqrt(e3^2*sin(theta(j))^2+e1^2*cos(theta(j))^2)*deltatheta;
    ycr(1,j)= 0;
    zcr(1,j)= -1.5*e1;
    %plot3(xcr(1,j),ycr(1,j),zcr(1,j),'b*')

    for i=2:length(psiaus)
        e1r = e1*sin(theta(j));
        e2r = e2*sin(theta(j));
        zcr(i,j) = -1.5*e1; % not necessary, just for visualization
        ycr(i,j) = ycr(i-1,j)+sqrt(e2r^2*sin(psiaus(i))^2+e1r^2*cos(psiaus(i))^2)*deltapsi;
        xcr(i,j) =   xcr(1,j);
        
    end
end

xcrtot = [xcr;xcr];
ycrtot = [ycr;-ycr];
zcrtot = [zcr,zcr];
k = convhull(xcrtot,ycrtot);
xprofile = xcrtot(k);
yprofile = ycrtot(k);

% plot the contact patch and the mobile platform
figure(2)
%plot(xcrtot,ycrtot,'b*',xprofile,yprofile,'r-')
fill(xprofile,yprofile,'c')
axis('equal')
hold on
title('Contact patch on plane xy (filled area) and mobile platform ')
xlabel('x (m)')
ylabel('y (m)')

bdmin =  2*(max(yprofile)+max(xprofile)/(ratioplat/2)/2);
bmin = bdmin/ratioplat;
bhmin = sqrt((bmin)^2-(bdmin/2)^2);

xb1min = bmin+bhmin;

plot([xb1min,0,0,xb1min],[0,bdmin/2,-bdmin/2,0],'ko-')
fill(xprofile,yprofile,'c')
axis([-0.002 0.03 -0.015 0.015])
legend('contact patch, rectified','mobile platform' )

% output: mobile platform dimension
b = max(bmin, bminfixed);
bd = b* ratioplat;
bh = sqrt(b^2 - (bd/2)^2);

% STEP 3.2 evaluation of fixed platform parameters
ad_min = e2+(bd-e2)*(e1+Hplat)/e1;
amin = ad_min/ratioplat;

% output: fixed platform dimension
a = max(amin,aminfixed);
ad = a* ratioplat;
ah = sqrt(a^2 - (ad/2)^2);


% platform vertex
% fixed platform in the fixed frame
a1 = [a 0 0]';
a2 = [-ah -ad/2 0]';
a3 = [-ah ad/2 0]';

% mobile platform in the mobile frame
b1_1 = [b 0 0]';
b2_1 = [-bh -bd/2 0]';
b3_1 = [-bh bd/2 0]';

% STEP 3.3 defining leg dimensions avoiding device singular configurations
figure(3)
% STEP 3.3.1 defining the set of feasible l1 and l2 value
 
for i = 1:length(psi)
    for j = 1:length(theta)
        %         %pz = pz0*(2-cos(psi(i)))/2;
        pz(i,j) = e1 + Hplat + e1/cos(psi(i));
        % % from these values, given the parallel mechanism constraints, evaluate the
        % % x and y displacements and the phi yaw angle
        % % calculate the phi yaw angle
        phi(i,j) = atan((b-bh)/bh*sin(theta(j))*sin(psi(i))/(cos(theta(j))+cos(psi(i))));
        % % calculate y displacement
        py(i,j) = -b * sin(phi(i,j))*cos(theta(j));
        % % calculate x displacement
        px(i,j) =bh*(cos(phi(i,j))*cos(theta(j))-sin(phi(i,j))*sin(theta(j))*sin(psi(i))...
            -cos(phi(i,j))*cos(psi(i)));
        %  mobile platform complete displacement
        p = [px(i,j) py(i,j) pz(i,j)]';
        Rroll = [1 0 0;
            0 cos(psi(i)) -sin(psi(i));
            0 sin(psi(i)) cos(psi(i))];
        Rpitch = [cos(theta(j)) 0 sin(theta(j));
            0 1 0;
            -sin(theta(j)) 0 cos(theta(j))];
        Ryaw = [cos(phi(i,j)) -sin(phi(i,j)) 0;
            sin(phi(i,j)) cos(phi(i,j)) 0;
            0 0 1];
        R = Ryaw*Rpitch*Rroll;
        %
        % mobile platform vertex coordinates in the fixed ref frame
        %
        b1  = p + R*b1_1;
        b2  = p + R*b2_1;
        b3  = p + R*b3_1;
        % %
        % evaluate leg unit vectors
        s1 = (b1-a1)/norm(b1-a1);
        s2 = (b2-a2)/norm(b2-a2);
        s3 = (b3-a3)/norm(b3-a3);
        %
        % evaluate fixed platform unit vectors
        q1 = a1/norm(a1);
        q2 = a2/norm(a2);
        q3 = a3/norm(a3);
        % %
        % % % evaluate leg angles
        beta1 = pi-acos(s1'*q1)
        beta2 = pi-acos(s2'*q2);
        beta3 = pi-acos(s3'*q3);
        % %
        % evaluate leg length
        d1 = norm(b1-a1);
        d2 = norm(b2-a2);
        d3 = norm(b3-a3);
        % store values
        d1m(i,j) = d1;
        d2m(i,j) = d2;
        d3m(i,j) = d3;
        
    end
end
% % 
dmin = min([min(min(d1m)) min(min(d2m)) min(min(d3m))]);
% %  
dmax = max([max(max(d1m)) max(max(d2m)) max(max(d3m))]);
% %  
% % 
alphamin = 0.01*pi;
alphamax = 0.99*pi;
% % 
l1v = [0:0.0002:0.04];
l2v = [0:0.0002:0.04];
% % % 
conmin1 = l1v*cos(alphamin)+ sqrt(l1v.^2*(cos(alphamin)^2-1)+dmin^2);
conmin2 = l1v*cos(alphamin)- sqrt(l1v.^2*(cos(alphamin)^2-1)+dmin^2);
% %  
conmax =  l1v*cos(alphamax)+ sqrt(l1v.^2*(cos(alphamax)^2-1)+dmax^2);
 %  
 % 
 % 
h = plot(l1v,conmin1,'r');
hold on
set(h,'LineWidth',3)
h = plot(l1v,conmax,'b');
set(h,'LineWidth',3)

hold on
hmin1 = area(l1v,conmin1);
set(hmin1,'FaceColor',[0.1,0.9,0.9],'LineWidth',0.01)
hmin2 = area(l1v,conmin2);
h = plot(l1v,conmin2,'r');
set(h(1),'LineWidth',3)
set(hmin2,'FaceColor',[1,1,1],'LineWidth',0.01)
hmin3 = area(l1v,conmax);
set(hmin3,'FaceColor',[1,1,1],'LineWidth',0.01)
h = plot(l1v,conmax,'b');
set(h(1),'LineWidth',3)
axis([0,0.04,0,0.04])


figure(3)
title('Filled area: l_1 and l_2 values allowing to avoid kinematics singularities  ')
xlabel('l_1  (m)')
ylabel('l_2  (m)')

% STEP 3.3.2 choose l1 and l2 automatically or manually by the user

disp('---------')
disp('How are l1 and l2 defined? ')
disp('1- Automatically, avoiding kinematics singularities and with a lower bound for l1 ')
disp('2- Manually selected by the user by picking a point on Figure 3')
disp(' ')
reply = input('Select an option (1/2 [1])','s');
if ischar(reply)== 0 || length(reply)~=1 || (reply ~= '1' && reply ~= '2')
    disp('Default option selected')
    reply = '1';
end

input('Press any key to continue');

if reply == '1'
    % automatically find the l1 l2 solution: pick l1 and l2 so that all the
    % constraints are satisfied and l1 is as small as possible
    [tmp,isol] = min(abs(conmin1 - conmax));
    l1tmp = l1v(isol);
    l2tmp = conmax(isol);
    % check hardware constraints
    if l1tmp<l1min
        [tmp,isolup] = min(abs(l1v-l1min));
        l1 = l1v(isolup);
        l2 = conmax(isolup);
    else
        l1 = l1tmp;
        l2 = l2tmp;
    end
else
    % the user manually selects l1 and l2 values by picking a value on the
    % diagram
    [l1,l2]= ginput(1);
    [tmp,checkcon1] = min(abs(l1-l1v));
    % a warning appears if the selected couple does not satisfy the
    % constraints
    if l2>conmin1(checkcon1) || l2<conmin2(checkcon1) || l2<conmax(checkcon1)
        disp('Warning - the selected configuration does not guarantee against singular configurations')
    elseif l1<l1min
        disp(['Warning - l_1 value is lower than the minimum constraint l_1min = ',num2str(l1min*1000),' mm'])
    end
end
% plot the selected l1,l2 couple on the diagram
plot(l1,l2,'r*','MarkerSize',10)

% collect the data for STEP 4 
a_mm = a * 1000;
b_mm = b * 1000;
ad_mm = ad * 1000;
bd_mm =bd * 1000;
l1_mm = l1 * 1000;
l2_mm = l2 * 1000;
%
clc 
disp('-------------------');
disp(' ');
disp('STEP 4 insert the personalized dimensions into OnShape');
disp(' ');
disp(['a = ',num2str(a_mm),' mm']);
disp(['b = ',num2str(b_mm),' mm']);
disp(['ad = ',num2str(ad_mm),' mm']);
disp(['bd = ',num2str(bd_mm),' mm']);
disp(['l1 = ',num2str(l1_mm),' mm']);
disp(['l2 = ',num2str(l2_mm),' mm']);
disp(' ');
disp('------------------');
disp(' ');
disp('Copy and paste the following lines into OnShape variable script');
disp(' ');
disp('------------------');
disp(' ');
disp(['var _a = ',num2str(a_mm),';']);
disp(['var _b = ',num2str(b_mm),';']);
disp(['var _ad = ',num2str(ad_mm),';']);
disp(['var _bd = ',num2str(bd_mm),';']);
disp(['var _l_1 = ',num2str(l1_mm),';']);
disp(['var _l_2 = ',num2str(l2_mm),';']);
disp(['var _e_1 = ',num2str(e1 * 1000),';']);
disp(['var _e_2 = ',num2str(e2 * 1000),';']);
disp(['var _e_3 = ',num2str(e3 * 1000),';']);
disp(' ');
disp('-------------------');
% ---------------------------------------------------
% Step 5 - device analysis in a specific configuration
%
% for this part of the script, please refer to
% F. Chinello, C. Pacchierotti, M. Malvezzi, D. Prattichizzo. 
% A Three Revolute-Revolute-Spherical wearable fingertip 
% cutaneous device for stiffness rendering. IEEE Transactions 
% on Haptics, 11(1):39-50, 2018.
%
% NOTE: in the following the pure kinematics is evaluated, the contact
% with the finger is not considered
% Device configuration - change these values to modify platform
% configuration
% z displacement of the mobile platform
% - this is the configuration in which the device touches the finger in the
% reference configuration - change it to test other configurations
pz = Hplat+2*e1;
 % psi roll angle - default configuration, change these values if you want
 % to simulate a different configuration.
psi = 0.;
 % theta pitch angle
 theta = 0.;
% 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % platform vertex
 % fixed platform in the fixed frame
 a1 = [a 0 0]';
 a2 = [-ah -ad/2 0]';
 a3 = [-ah ad/2 0]';
% 
 % mobile platform in the mobile frame
 b1_1 = [b 0 0]';
 b2_1 = [-bh -bd/2 0]';
 b3_1 = [-bh bd/2 0]';
% 
 % Complete platform configuration        
 % Given the parallel mechanism constraints, evaluate the
 % x and y displacements and the phi yaw angle
 % calculate the phi yaw angle
 phi = atan((b-bh)/bh*sin(theta)*sin(psi)/(cos(theta)+cos(psi)));
% % calculate y displacement
 py = -b*sin(phi)*cos(theta);
% % calculate x displacement
 px =bh*(cos(phi)*cos(theta)-sin(phi)*sin(theta)*sin(psi)-cos(phi)*cos(psi));
% 
 % mobile platform complete displacement
 p = [px py pz]';
 Rroll = [1 0 0;
     0 cos(psi) -sin(psi);
     0 sin(psi) cos(psi)];
 Rpitch = [cos(theta) 0 sin(theta);
     0 1 0;
     -sin(theta) 0 cos(theta)];
 Ryaw = [cos(phi) -sin(phi) 0;
     sin(phi) cos(phi) 0;
     0 0 1];
 R = Ryaw*Rpitch*Rroll;
%         
 % mobile platform vertex coordinates in the fixed ref frame
% 
 b1  = p + R*b1_1;
 b2  = p + R*b2_1;
 b3  = p + R*b3_1;
% 
% 
% 
% evaluate leg unit vectors
 s1 = (b1-a1)/norm(b1-a1);
 s2 = (b2-a2)/norm(b2-a2);
 s3 = (b3-a3)/norm(b3-a3);
% 
 % evaluate fixed platform unit vectors
 q1 = a1/norm(a1);
 q2 = a2/norm(a2);
 q3 = a3/norm(a3);
% 
 % evaluate leg angles
 beta1 = pi-acos(s1'*q1);
 beta2 = pi-acos(s2'*q2);
 beta3 = pi-acos(s3'*q3);
% 
 % evaluate leg length
 d1 = norm(b1-a1);
 d2 = norm(b2-a2);
 d3 = norm(b3-a3);
% 
 % evaluate leg  angles
 gamma1 = acos((l1^2+d1^2-l2^2)/(2*l1*d1));
 gamma2 = acos((l1^2+d2^2-l2^2)/(2*l1*d2));
 gamma3 = acos((l1^2+d3^2-l2^2)/(2*l1*d3));
% 
 % evaluate leg base angle
 alpha1 = atan((b1(3)-a1(3))/sqrt((b1(1)-a1(1))^2+(b1(2)-a1(2))^2));
 alpha2 = atan((b2(3)-a2(3))/sqrt((b2(1)-a2(1))^2+(b2(2)-a2(2))^2));
 alpha3 = atan((b3(3)-a3(3))/sqrt((b3(1)-a3(1))^2+(b3(2)-a3(2))^2));
% 
 % evaluate actuator rotation angles
 % this is the output of the inverse kinematics of the mechanism
 delta1 = pi-alpha1-gamma1;
 delta2 = pi-alpha2-gamma2;
 delta3 = pi-alpha3-gamma3;
%         
 % evaluate knee position coordinates in the local vertex frames
 k1_1 = [a+l1*cos(delta1) 0 l1*sin(delta1)]';
 k2_2 = [a+l1*cos(delta2) 0 l1*sin(delta2)]';
 k3_3 = [a+l1*cos(delta3) 0 l1*sin(delta3)]';
% 
 % local vertex frame rotations
 chi1 = atan2(a1(2),a1(1));
 chi2 = atan2(a2(2),a2(1));
 chi3 = atan2(a3(2),a3(1));
 Rchi1 = [cos(chi1) -sin(chi1) 0;
     sin(chi1) cos(chi1) 0;
     0 0 1];
 Rchi2 = [cos(chi2) -sin(chi2) 0;
     sin(chi2) cos(chi2) 0;
     0 0 1];
 Rchi3 = [cos(chi3) -sin(chi3) 0;
     sin(chi3) cos(chi3) 0;
     0 0 1];
 
 % knee points in the global frame
 k1 = Rchi1*k1_1;
 k2 = Rchi2*k2_2;
 k3 = Rchi3*k3_3;
% 
 % joint unit vectors
 kvers = [0 0 1]';
 a1vers = [cos(chi1) sin(chi1) 0]';
 a2vers = [cos(chi2) sin(chi2) 0]';
 a3vers = [cos(chi3) sin(chi3) 0]';
% 
 u1vers = cross(kvers,a1vers);
 u2vers = cross(kvers,a2vers);
 u3vers = cross(kvers,a3vers);
% 

 % plot - device scheme -------
figure(1) % in this case plot the device with the finger, change the number of
% figure to create a new image
axis('equal')
plot3([a1(1) a2(1) a3(1) a1(1)]+0.005,[a1(2) a2(2) a3(2) a1(2)],[a1(3) a2(3) a3(3) a1(3)]+Hplat+e1,'r','LineWidth',2)
hold on
plot3([b1(1) b2(1) b3(1) b1(1)]+0.005,[b1(2) b2(2) b3(2) b1(2)],Hplat+e1-[b1(3) b2(3) b3(3) b1(3)],'b','LineWidth',2)
% plot the legs
plot3([a1(1) k1(1) b1(1)]+0.005,[a1(2) k1(2) b1(2)],Hplat+e1-[a1(3) k1(3) b1(3)],'k','LineWidth',2)
plot3([a2(1) k2(1) b2(1)]+0.005,[a2(2) k2(2) b2(2)],Hplat+e1-[a2(3) k2(3) b2(3)],'k','LineWidth',2)
plot3([a3(1) k3(1) b3(1)]+0.005,[a3(2) k3(2) b3(2)],Hplat+e1-[a3(3) k3(3) b3(3)],'k','LineWidth',2)
axis('equal')
% axis([-0.02 0.04 -0.03 0.03 -0.02 0.03])
% add some text to the figure
text(a1(1)+0.005,a1(2),a1(3)+Hplat+e1+0.002,'A_1','FontSize',18)
text(a2(1)+0.005,a2(2),a2(3)+Hplat+e1+0.002,'A_2','FontSize',18)
text(a3(1)+0.005,a3(2),a3(3)+Hplat+e1+0.002,'A_3','FontSize',18)
text(b1(1)+0.005,b1(2),-b1(3)+Hplat+e1-0.002,'B_1','FontSize',18)
text(b2(1)+0.005,b2(2),-b2(3)+Hplat+e1-0.002,'B_2','FontSize',18)
text(b3(1)+0.005,b3(2),-b3(3)+Hplat+e1-0.002,'B_3','FontSize',18)
text(k1(1)+0.005,k1(2),-k1(3)+Hplat+e1+0.002,'D_1','FontSize',18)
text(k2(1)+0.005,k2(2),-k2(3)+Hplat+e1+0.002,'D_2','FontSize',18)
text(k3(1)+0.005,k3(2),-k3(3)+Hplat+e1+0.002,'D_3','FontSize',18)
% evaluate an approximation of device volume, defined as the volume of the
% comvex hull of the main points (Ai, Bi,Di) 
mpoint = [a1 a2 a3 b1 b2 b3 k1 k2 k3];
DT = delaunayTriangulation(mpoint');
[tmp,v_device] = convexHull(DT);
% approximation of finger volume, the finger is approximated as a half
% ellipsoid and a cylinder with elliptical base 
v_finger = 2/3*pi*e1*e2*e3+pi*e1*e2*e3
% volume ratio, ratio between device and finger volume.
vratio = v_device/v_finger

plot3([a1(1) a2(1) a3(1) a1(1)]+0.005,[a1(2) a2(2) a3(2) a1(2)],[a1(3) a2(3) a3(3) a1(3)]+Hplat+e1,'r','LineWidth',2)
hold on
plot3([b1(1) b2(1) b3(1) b1(1)]+0.005,[b1(2) b2(2) b3(2) b1(2)],Hplat+e1-[b1(3) b2(3) b3(3) b1(3)],'b','LineWidth',2)
% plot the legs
plot3([a1(1) k1(1) b1(1)]+0.005,[a1(2) k1(2) b1(2)],Hplat+e1-[a1(3) k1(3) b1(3)],'k','LineWidth',2)
plot3([a2(1) k2(1) b2(1)]+0.005,[a2(2) k2(2) b2(2)],Hplat+e1-[a2(3) k2(3) b2(3)],'k','LineWidth',2)
plot3([a3(1) k3(1) b3(1)]+0.005,[a3(2) k3(2) b3(2)],Hplat+e1-[a3(3) k3(3) b3(3)],'k','LineWidth',2)
axis('equal')
% axis([-0.02 0.04 -0.03 0.03 -0.02 0.03])
% add some text to the figure
text(a1(1)+0.005,a1(2),a1(3)+Hplat+e1+0.002,'A_1','FontSize',18)
text(a2(1)+0.005,a2(2),a2(3)+Hplat+e1+0.002,'A_2','FontSize',18)
text(a3(1)+0.005,a3(2),a3(3)+Hplat+e1+0.002,'A_3','FontSize',18)
text(b1(1)+0.005,b1(2),-b1(3)+Hplat+e1-0.002,'B_1','FontSize',18)
text(b2(1)+0.005,b2(2),-b2(3)+Hplat+e1-0.002,'B_2','FontSize',18)
text(b3(1)+0.005,b3(2),-b3(3)+Hplat+e1-0.002,'B_3','FontSize',18)
text(k1(1)+0.005,k1(2),-k1(3)+Hplat+e1+0.002,'D_1','FontSize',18)
text(k2(1)+0.005,k2(2),-k2(3)+Hplat+e1+0.002,'D_2','FontSize',18)
text(k3(1)+0.005,k3(2),-k3(3)+Hplat+e1+0.002,'D_3','FontSize',18)
axis([-0.05 0.05 -0.03 0.03 -0.015 0.03])

% evaluate an approximation of device volume, defined as the volume of the
% comvex hull of the main points (Ai, Bi,Di) 
mpoint = [a1 a2 a3 b1 b2 b3 k1 k2 k3];
DT = delaunayTriangulation(mpoint');
[tmp,v_device] = convexHull(DT);
% approximation of finger volume, the finger is approximated as a half
% ellipsoid and a cylinder with elliptical base 
v_finger = 2/3*pi*e1*e2*e3+pi*e1*e2*e3;
% volume ratio, ratio between device and finger volume.
vratio = v_device/v_finger;
