% example of spring-loaded inverted pendulum 
% find a spring such that there is a stable equilibrium
% Equation of motion is -k*phi+m*g*l*sin(phi) = J d2phi/dt^2
% scale with m=1 g=1 l=1
% -k*phi+sin(phi) = J d2phi/dt^2
% equilibrium phi = constant -> d2phi/dt^2 = 0 -> sin(phi) = k*phi 
% and stable  when k > cos(phi)

% set the default linewidth
set(0,'DefaultLineLineWidth',2)
% remove by
%set(0,'DefaultLineLineWidth','remove')

m=6;
phi=linspace(0,m*pi,m*100+1);
% find the stiffness k for equilibrium at an angle phi
k=sin(phi)./phi;
% lim phi->0 sin(phi)/phi = 1
k(1)=1;

f1=figure;
plot(phi./pi,k);
hold on;
plot([0 0],[-.5 2])
ylabel('stiffness k')
xlabel('\phi/\pi')
title('Equilibrium for k = sin(\phi)/\phi')
%ylim([0 1.4]);
grid

%return
%% for stability we have k > cos(phi)
% plot k=cos(pi)
kstable=cos(phi);
plot(phi./pi,kstable,'r')
title('Equilibrium for k = sin(\phi)/\phi, Stable for k > cos(\phi)')

%return
%% make a new plot with solid lines for stable solutions
% and dashed lines for unstable solutions

lunstable=(k<kstable);
kstable=k;
kstable(lunstable)=NaN;
f2=figure
kunstable=k;
kunstable(~lunstable)=NaN;
plot(phi./pi,kstable,'-b',phi./pi,kunstable,'--r');
hold on
plot([0 0],[1 1.2])
plot([0 0],[0 1],'--r')
ylabel('stiffness k')
xlabel('\phi/\pi')
title('Bifurcation of the Equilibria of a spring-loaded inverted pendulum')
ylim([0 1.2]);
grid
legend('stable','unstable')

%return
%% find the first zero stability crossing
% where k=cos(phi) -> sin(phi)/phi=cos(phi)
% this is around 2.5*pi
phi0=fzero('sin(x)./x-cos(x)',[2*pi 3*pi])
k0=sin(phi0)/phi0;
plot([phi0 phi0]./pi,[0,k0],':k','LineWidth',0.5)
text(phi0/pi-0.2,-0.05,sprintf('%g',phi0/pi))

%return
%% the next is aprox at 4.5*pi 
phi0=fzero('sin(x)./x-cos(x)',[4*pi 5*pi])
k0=sin(phi0)/phi0;
plot([phi0 phi0]./pi,[0,k0],':k','LineWidth',0.5)
text(phi0/pi-0.2,-0.05,sprintf('%g',phi0/pi))
