Problem with simulating passive dynamic walker with knees

Hi, I trying to simulate the Passive dynamic walker with knees. I have simulated the straight leg walker with the code from Dynamic Walking Matlab simulation guide site from cornell. I have adapted the same code to incorporate knees. As the straight leg walker has a heelstrike collison event when one feet touches the ground the kneed walker also has a kneestrike event where the lower leg and the upper leg get aligned. The problem i am facing is that the while finding the fixed points for the diff equations of the walker the fsolve function doesnt converge. I am trying to find out the error, as i have used the equations straight from the Hsu chen's thesis of Passive dynamic walking with knees. I am new to matlab and would appreciate any help. I have attached the code and the site for reference. Dynamic Walking simulation guide

%Passive dynamic walker with knees%
%Last modified 15/10/2019
% Abhinash


function knee

clc 
clear all
close all
format long

M = 0.5; %mass of hip
mt = 0.5; %mass of thigh
ms =  0.05; %mass of lower leg
L = 1; %lenght of total leg
b1 =0.125; %lenght knee joint to COM of lower leg
a1 =0.375; %lenght foot to COM of lower leg
b2 =0.325; %lenght hip to COM of thigh
a2 =0.175;%lenght knee joint to COM of thigh
g=9.81;
gam = 0.0504; %slope in rad

steps = 5;

GL_DIM = [ M mt ms L b1 a1 b2 a2 gam g];

q01 = 0.1877;    w01 = -1.1014; %negative angles mean the leg is to the left of vertical
q02 =-0.2884 ;    w02 =-0.0399 ;
q03 =-0.2884;    w03 =-0.0399 ;

z0 = [q01 w01 q02 w02 q03 w03];

%%%%% Root finding, Period one gait %%%%
options = optimset('TolFun',1e-6,'TolX',1e-6,'Display','off');
[zstar,fval,exitflag,output,jacob] = fsolve(@fixedpt,z0,options,GL_DIM);
if exitflag == 1
    disp('Fixed points are');
    zstar
else
    error('Root finder not converged, change guess or change system parameters')
end


%%%% Stability, using linearised eigenvalue %%%
J=partialder(@onestep,zstar,GL_DIM);
disp('EigenValues for linearized map are');
eig(J)

[z,t]=onestep(z0,GL_DIM,steps);

q1 = z(:,1);
q2 = z(:,3); 
q3 = z(:,5);

%Plotting
figure(1);
axis([-2 2 -2 2]);

for i=1:length(t)

xh=L*sin(q1(i));
yh=L*cos(q1(i));          %hip coordinates

xn1 = xh + (a2+b2)*sin(q1(i));
yn1 =- yh + (a2+b2)*cos(q1(i));  % right knee coordinates

xn2 = xh - (a2+b2)*sin(q2(i));
yn2 = -yh + (a2+b2)*cos(q2(i));  % left knee coordinates

xp1 =  xh + L*sin(q1(i));
yp1 =  -yh + L*sin(q1(i));           %right foot coordinates

xp2 =  xh - (a1+b1)*sin(q3(i));
yp2 =  -yh + (a1+b1)*sin(q3(i));         %left foot coordinates

figure(1)
plot([xh xp1],[yh yp1]);
 % plots right half of walker
hold on
plot ([xh xn2],[yh yn2]);
plot ([xn2 xp2],[yn2 yp2]); % plots left half of walker

plot([xh-1 xh+1],[0 0],'black')%plots the ground line

axis([xh-1 xh+1 -1 1.5]);
axis off;
hold off;

end 

%%%%Functions

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function zdiff=fixedpt(z0,GL_DIM)
zdiff=onestep(z0,GL_DIM)-z0; 

function J=partialder(FUN,z,GL_DIM)
pert=1e-5;
 
%%% Using central difference, accuracy quadratic %%%
for i=1:length(z)
    ztemp1=z; ztemp2=z;
    ztemp1(i)=ztemp1(i)+pert; 
    ztemp2(i)=ztemp2(i)-pert; 
    J(:,i)=(feval(FUN,ztemp1,GL_DIM)-feval(FUN,ztemp2,GL_DIM)) ;
end
J=J/(2*pert);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z,t]=onestep(z0,GL_DIM,steps)

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);

flag = 1;
if nargin<2
    error('need more inputs to onestep');
elseif nargin<3
    flag = 0; %send only last state
    steps = 1;
end

t0 = 0; 
dt = 5;
t_ode = t0;
z_ode = z0;

for i=1:steps
    options=odeset('abstol',2.25*1e-14,'reltol',2.25*1e-14,'events',@collision);
    tspan = linspace(t0,t0+dt,1000);
    [t_temp1, z_temp1, tfinal1] = ode113(@ranger_ss_simplest_3link,tspan,z0,options,GL_DIM);
    
    zkneeplus=kneestrike_ss_simplest(t_temp1(end),z_temp1(end,:),GL_DIM);
    
    options2=odeset('abstol',2.25*1e-14,'reltol',2.25*1e-14,'events',@collision2);
    
    [t_temp2, z_temp2, tfinal2] = ode113(@ranger_ss_simplest,tspan,zkneeplus,options2,GL_DIM);

    zplus=heelstrike_ss_simplest(t_temp2(end),z_temp2(end,:),GL_DIM);
    
    t_temp = [t_temp1; t_temp2];
    z_temp = [z_temp1; z_temp2];
    
    z0 = zplus;
    t0 = t_temp(end);
    
    %%% dont include the first point
    t_ode = [t_ode; t_temp(2:end); t0];
    z_ode = [z_ode; z_temp(2:end,:); z0];
    
end

z = zplus;

if flag==1
   z=z_ode;
   t=t_ode;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function zdot = ranger_ss_simplest_3link(t,z,GL_DIM)

q1=z(1);
w1=z(2);
q2=z(3);
w2=z(4);
q3=z(5);
w3=z(6);

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);
                                   

H11 = ms*a1*a1 + mt*(a1+b1+a2)^2 +(M+ms+mt)*L*L;
H12 = -(mt*b2+ms*(a2+b2))*L*cos(q2-q1);
H13 = -ms*b1*L*cos(q3-q1);
H22 = mt*b2*b2 + ms*(a2+b2)^2;
H23 = ms*(a2+b2)*b1*cos(q3-q2);
H33 = ms*b1*b1;

H = [ H11 H12 H13
      H12 H22 H23
      H13 H23 H33 ];

h11 = 0;
h12 = -(mt*b2 + ms*(a2+b2))*L*sin(q1-q2)*w2;
h13 = -ms*b1*L*sin(q1-q3)*w3;
h21 = (mt*b2 + ms*(a2+b2))*L*sin(q1-q2)*w1;
h22 = 0;
h23 = ms*(a2+b2)*b1*sin(q3-q2)*w3;
h31 = ms*b1*L*sin(q1-q3)*w1;
h32 = -ms*(a2+b2)*b1*sin(q3-q2)*w2;
h33 = 0;

B = [ h11 h12 h13
      h21 h22 h23
      h31 h32 h33 ];
  
g1 = -(ms*a1 + mt*((a1+b1)+a2)+ (M+ms+mt)*L)*g*sin(q1) ;
g2 = (mt*b2+ ms*(a2+b2))*g*sin(q2);
g3 =  ms*b1*g*sin(q3);

G = [ g1; g2; g3];

qdot = [ w1; w2; w3];

q = [ q1; q2; q3];

X = -B*qdot - G;

qddot = H\X ;                                 

alp1 = qddot(1);                                       
alp2 = qddot(2);                                      
alp3 = qddot(3);                                       

zdot = [w1 alp1 w2 alp2 w3 alp3]';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function zdot = ranger_ss_simplest(t,z,GL_DIM)

q1=z(1);
w1=z(2);
q2=z(3);
w2=z(4);
q3=z(5);
w3=z(6);

Ta=0;
Th=0;
Thip=0;

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);
                                   
H11 = ms*a1*a1 + mt*(a1+b1+a2)^2 +(M+ms+mt)*L*L;
H12 = -(mt*b2+ms*(a2+b2+b1))*L*cos(q2-q1);
H22 = mt*b2*b2 + ms*(a2+b2+b1)^2;     

H = [H11 H12
     H12 H22];

h11 = 0;
h12 =  -(mt*b2 + ms*(a2+b2+b1))*L*sin(q1-q2)*w2;
h21 =  (mt*b2 + ms*(a2+b2))*L*sin(q1-q2)*w1;
h22 = 0;

B = [h11 h12
     h21 h22];
g1 = -(ms*a1 + mt*((a1+b1)+a2)+ (M+ms+mt)*L)*g*sin(q1);
g2 = (mt*b2+ ms*(a2+b2+b1))*g*sin(q2);

G = [g1; g2];

qdot = [ w1; w2];

q = [ q1; q2];

qddot = H \ (-B*qdot - G);                                    

alp1 = qddot(1);                                       
alp2 = qddot(2);                                      
alp3 = qddot(2);                                       

zdot = [w1 alp1 w2 alp2 w3 alp3]';



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function zkneeplus=kneestrike_ss_simplest(t,z,GL_DIM)      

q1 = z(1);     w1 = z(2);
q2 = z(3);     w2 = z(4);
q3 = z(5);     w3 = z(6);

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);                                                      


qm = [w1; w2; w3];

Qm11 = -(ms*(a2+b2) + mt*b2)*L*cos(q1-q2)-ms*b1*L*cos(q1-q3)+(mt+ms+M)*L*L+ms*a1*a1+mt*(a1+b1+a2)^2;
Qm12 = -(ms*(a2+b2) + mt*b2)*L*cos(q1-q2)+ms*b1*L*cos(q2-q3)+mt*b2*b2+ms*(a2+b2)^2;
Qm13 = - ms*b1*L*cos(q1-q2) + ms*b1*(a2+b2)*cos(q2-q3)+ms*b1*b1;
Qm21 = -(ms*(a2+b2) + mt*b2)*L*cos(q1-q2)-ms*b1*L*cos(q2-q3);
Qm22 = ms*b1*(a2+b2)*cos(q2-q3)+ms*(a2+b2)^2+mt*b2*b2;
Qm23 = ms*b1*(a2+b2)*cos(q2-q3)+ms*b1*b1;

Qm = [ Qm11 Qm12 Qm13
       Qm21 Qm22 Qm23];

Qp21 = -(ms*(b1+a2+b2)+mt*b2)*L*cos(q1-q2);
Qp11 = Qp21 + mt*(a1+b1+a2)^2+(M+mt+ms)*L*L+ms*a1*a1;
Qp12 = Qp21 + ms*(a2+b2+b1)^2 +mt*b2*b2;
Qp22 = ms*(a2+b2+b1)^2 +mt*b2*b2;

Qp = [Qp11 Qp12
      Qp21 Qp22];

wp = Qp\(Qm*qm) ;

w_1 = wp(1);
w_2 = wp(2);
w_3 = w_2;

zkneeplus=[q1 w_1 q2 w_3 q3 w_3];

function zplus=heelstrike_ss_simplest(t,z,GL_DIM)      

q1 = z(1);     w1 = z(2);
q2 = z(3);     w2 = z(4);
q3 = z(5);     w3 = z(6);

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);                                                      

qminus = [q1; q2];
qm = [w1; w2];
P = [0 1; 1 0; 1 0];

qplus = P*qminus;

Qm12 = -ms*a1*(a2+b2+b1)+mt*b2*(a1+b1+a2);
Qm11 = Qm12 + (M*L + 2*mt*(a2+a1+b1)+ms*a1)*L*cos(q1-q2);
Qm21 =  Qm12;
Qm22 = 0;

Qm = [Qm11 Qm12
      Qm21 Qm22];
  
Qp21 = -(ms*(b1+a2+b2)+mt*b2)*L*cos(q1-q2);
Qp11 = Qp21 + mt*(a1+b1+a2)^2+(M+mt+ms)*L*L+ms*a1*a1;
Qp12 = Qp21 + ms*(a2+b2+b1)^2 +mt*b2*b2;
Qp22 =ms*(a2+b2+b1)^2 +mt*b2*b2;

Qp = [Qp11 Qp12
      Qp21 Qp22];

wp = Qp \ (Qm*qm) ;

w_1 = wp(1);
w_2 = wp(2);
w_3 = w_2;

zplus = [qplus(1) w_1 qplus(2) w_2 qplus(3) w_3];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%This is the kneestrike collison
function [val, isterminal,direction]=collision(t,z,GL_DIM)

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);

q1 = z(1); q2 = z(3); 
q3 = z(5); 

val = q3-q2;
isterminal=1; %Ode should terminate is conveyed by 1, if you put 0 it goes till the final time u specify
direction=-1; % The t_final can be approached by any direction is indicated by this
end

function [val, isterminal,direction]=collision2(t,z,GL_DIM)

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);

q1 = z(1); q2 = z(3); 
q3 = z(5); 

val = L*cos(q1)-L*cos(q2);
if(q2>-0.025) 
  isterminal=0;
else
  isterminal=1;
end %Ode should terminate is conveyed by 1, if you put 0 it goes till the final time u specify
direction=1; % The t_final can be approached by any direction is indicated by this

[Phaneesha Chilaveni]

Hello sir I need a help from you.....I want the code for the straight leg walker ,can I get it???