% stiffnesstheory.m
% Author: Eric Nickel

% This code is designed to establish graphical analysis of the theory
% behind the variable stiffness joint

% Setting L1 (the distance from the axis of rotation to the base plate
% insertion point) to 1 normalizes all lengths with respect to the actual
% L1 length
L1 = 1;

% L2 (the distance from the axis of rotation to the rotating member
% insertion point) is dependent on the desired stiffness, and is the
% measure which is adjusted by the user
L2 = 0:3/50:3;

% theta is the angle range of the rotating member with respect to the
% vertical.  The anatomical range of motion for the ankle joint during
% normal walking is -15 to +20 degrees, so we will look at the range from
% -20 to +30 degrees (-pi/9 to pi/6 radians)
theta = -pi/9:pi/180:pi/6;

% The height (h) of the rotating insertion point with respect to the base 
% plate and the lateral displacement of the rotating insertion point (a)
% are functions of the angle and rotating insertion point distance (L2)
h = L2'*cos(theta);
a = L1+L2'*sin(theta);

% Show the relationships between height changes and lateral position
% changes as a result of angular changes

figure(1)
subplot(1,2,1);
surf(theta,L2,h);
title('Height vs theta and L2')
xlabel('theta [rad]')
ylabel('L2/L1')
zlabel('Height/L1')
grid on

subplot(1,2,2);
surf(theta,L2,a)
title('Displacement vs theta and L2')
xlabel('theta [rad]')
ylabel('L2/L1')
zlabel('Displacement/L1')

figure(2)
subplot(1,2,1);
plot(theta,h(6,:),theta,h(17,:),theta,h(18,:),theta,h(40,:));
title('Height vs theta (L2 = L1)')
axis([-0.4 0.6 0 2.5])
legend('L2 = 0.36 * L1','L2 = 0.96 * L1','L2 = 1.02 * L1','L2 = 2.40 * L1')
xlabel('theta [rad]')
ylabel('Height/L1')
grid on

subplot(1,2,2);
plot(theta,a(6,:),theta,a(17,:),theta,a(18,:),theta,a(40,:));
title('Displacement vs theta (L2 = L1)')
axis([-0.4 0.6 0 1.6])
legend('L2 = 0.36 * L1','L2 = 0.96 * L1','L2 = 1.02 * L1','L2 = 2.40 * L1')
xlabel('theta [rad]')
ylabel('Displacement/L1')
grid on

% L3 is the distance between the base plate insertion point and the
% rotating member insertion point.  Note that angular position change is
% represented by moving along COLUMNS while rotating member insertion point
% changes (L2 changes) are represented by moving along ROWS.
L3 = ( (L2'*cos(theta)).^2 + (L1+L2'*sin(theta)).^2 ).^0.5;

% To determine the actual length change of the spring for a given angle,
% assume the set-point is positioned such that at the starting angle (-20
% degrees) is the neutral point for the spring.  This means that the amount
% of extension the spring experiences is the instantaneous value of L3
% minus the starting value of L3.

% Establish the initial values for spring+spacer length (note that we used 
% a cable/spool system to provide/reel in slack on a cable to allow the
% spring to be at the neutral length for any starting L2)

% Iterate a loop to subtract the starting L3 values from the current L3
% values for every angular position
for col = 1:51
      L3scaled(:,col) = L3(:,col)-L3(:,1);
end

% Make the plots and make them look nice
figure(3)
surfc(theta,L2,L3scaled)
title('Spring Stretch vs theta and L2')
xlabel('theta [rad]')
ylabel('L2/L1')
zlabel('Spring Stretch / L1')

% Plot the amount of spring stretch over a range of shank angles given some
% L2/L1 ratios
figure(4)
subplot(1,2,1);
plot(theta,L3scaled(5,:),theta,L3scaled(10,:),theta,L3scaled(20,:),theta,L3scaled(51,:));
title('Spring Stretch vs theta')
legend('L2/L1 = 0.3','L2/L1 = 0.6','L2/L1 = 1.2','L2/L1 = 3.0')
xlabel('theta [rad]')
ylabel('Spring Stretch/L1')
grid on

% Plot the amount of spring stretch over a range of L2/L1 ratios for some
% given shank angles
subplot(1,2,2);
plot(L2,L3scaled(:,6),L2,L3scaled(:,11),L2,L3scaled(:,21),L2,L3scaled(:,51));
title('Spring Stretch vs L2')
legend('theta = -15 degrees','theta = -10 degrees','theta = 0 degrees','theta = 30 degrees')
xlabel('L2/L1')
ylabel('Spring Stretch/L1')
grid on

% The torque at any given angle is the portion of the spring tension force
% acting tangentially to the instantaneous rotation

% theta2 is the angle of the spring tension force with respect to the
% rotating member such that sin(theta2) is the tangential torque producing
% force and cos(theta2) is the force acting along the axis of the rotating
% member (producing compression)

for row = 1:51
    for col = 1:51
        ang = atan(h(row,col)/a(row,col));
        theta2(row,col) = pi/2 - theta(col) - ang;
    end
end

% The spring tension force is the product of the spring displacement
% (L3scaled) and the spring stiffness (k, obtained from spring specs).  The
% torque acting on the rotating member is the product of the tangential
% force (k*L3scaled*sin(theta2)) and the distance from the axis of
% rotation (L2), so
T = (L3scaled.*sin(theta2));
for col = 1:51
    T(:,col) = L2(col)*T(:,col);
end


% Plot a graph of the applied torque for a given shank angle and L2
% Note that the maximum torque value occurs at L2 = L1.  Note that this
% really super-cool graph is the bread and butter of this entire program.
% All of the other code is to supplement this plot.  The use of this plot
% is to find the region of L2/L1 ratios over which we can effectively
% control the rotational stiffness of the joint.
figure(5)
surfc(theta,L2,T)
title('Torque vs theta and L2')
xlabel('theta [rad]')
ylabel('L2/L1')
zlabel('Torque / L1^2')

% Plot a graph of the applied torque over different L2/L1 ratios for given
% shank angles
figure(6)
plot(L2,T(:,6),L2,T(:,11),L2,T(:,21),L2,T(:,51));
title('Torque vs L2/L1')
legend('theta = -15 degrees','theta = -10 degrees','theta = 0 degrees','theta = 30 degrees')
xlabel('L2/L1')
ylabel('Torque Resistance')
grid on