% powered_walker.m
% last modified 04/15/09
% Josh Petersen


function powered_walker

clc
clear all
close all
format long

HIPSWING = 1; HIPFREE = 2;

global HIPSTATE;
HIPSTATE=HIPSWING;


M = 1.0; m = 0.5; I = 0.02; l = 1.0; w = 0.0; 
c = 0.5; r = 0.2; d = 0.00; g = 1.0; gam = 0.00; 

steps=5;

GL_DIM = [M m c I g l w r d gam];

q10=.2; u10=-.2;
q20=pi; u20=0;
q30=.4; u30=-.4;
q40=pi; u40=0;

z0=[q10 u10 q20 u20 q30 u30 q40 u40];


%%%%% 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

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

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


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

for i=1:length(t)

xh=l*sin(q1(i)+q2(i))-d*sin(q1(i))-r*q1(i);
yh=-l*cos(q1(i)+q2(i))+d*cos(q1(i))+r;
xa1=xh-sin(q1(i)+q2(i));
ya1=yh+cos(q1(i)+q2(i));
th1=linspace(q1(i)-pi/2+pi/4,q1(i)-pi/2-pi/4,50);
th2=linspace(q1(i)-q3(i)-pi/2+pi/4,q1(i)-q3(i)-pi/2-pi/4,50);

xa2=xh+l*sin(q3(i)-q1(i)-q2(i));
ya2=yh+l*cos(q3(i)-q1(i)-q2(i));

xf1=xa1+r*cos(q1(i)-pi/2);
yf1=ya1+r*sin(q1(i)-pi/2);

xf2=xa2+r*cos(q1(i)-q3(i)-pi/2);
yf2=ya2+r*sin(q1(i)-q3(i)-pi/2);

rx1=xa1+r*cos(th1);
ry1=ya1+r*sin(th1);
rx2=xa2+r*cos(th2);
ry2=ya2+r*sin(th2);

figure(1)
plot([xh xa1 xf1],[yh ya1 yf1],'LineWidth',2);%plots the right half of the walker
hold on;
plot([xh xa2 xf2],[yh ya2 yf2],'LineWidth',2);% plots the left half
plot([xh-1 xh+1],[0 0],'black')%plots the ground line
plot([rx1],[ry1],'LineWidth',2);
plot([rx2],[ry2],'LineWidth',2);

axis([xh-1 xh+1 -.25 1.75]);
axis off;
hold off;

end

figure(2)
plot(t,q1,t,q3);
xlabel('Time (s)');
ylabel('Angle (rad)');
legend('Stance Angle','Swing Angle')

%%%%Functions

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

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

M = GL_DIM(1);  m = GL_DIM(2); c = GL_DIM(3);   
I = GL_DIM(4);  g = GL_DIM(5); l = GL_DIM(6);   
w = GL_DIM(7);  r = GL_DIM(8); d = GL_DIM(9);   
gam = 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_temp, z_temp, tfinal] = ode113(@ranger_ss_simplest,tspan,z0,options,GL_DIM);

    zplus=heelstrike_ss_simplest(t_temp(end),z_temp(end,:),GL_DIM);
    
    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(1:2) pi 0 zplus(5:6) pi 0];

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function torque = controller(t,z,GL_DIM)
    
HIPSWING = 1; HIPFREE = 2;
P = 0.1; D = 0.5; q3REF = 0; 
   
M = GL_DIM(1);  m = GL_DIM(2); c = GL_DIM(3);   
I = GL_DIM(4);  g = GL_DIM(5); l = GL_DIM(6);   
w = GL_DIM(7);  r = GL_DIM(8); d = GL_DIM(9);   
gam = GL_DIM(10);

q1=z(1); u1=z(2);
q2=z(3); u2=z(4);
q3=z(5); u3=z(6);
q4=z(7); u4=z(8);


if (q3-q3REF)<=0
    HIPSTATE=HIPFREE;
else
    HIPSTATE=HIPSWING;
end

if HIPSTATE == HIPSWING
    torque=- P*(q3-q3REF) - D*u1;
else
    torque =0;
end


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

function zdot = ranger_ss_simplest(t,z,GL_DIM)

q1=z(1);
u1=z(2);
q2=z(3);
u2=z(4);
q3=z(5);
u3=z(6);
q4=z(7);
u4=z(8);

Ta=0;
Th=controller(t,z,GL_DIM);
Thip=0;

M = GL_DIM(1);  m = GL_DIM(2); c = GL_DIM(3);   
I = GL_DIM(4);  g = GL_DIM(5); l = GL_DIM(6);   
w = GL_DIM(7);  r = GL_DIM(8); d = GL_DIM(9);   
gam = GL_DIM(10);                                       

M11 = 2*M*d*l*cos(q2)-2*I-2*m*l^2-2*m*w^2-2*m*d*c*cos(-q3+q2)+2*m*d*w*sin(-q3+q2)+2*m*l*c*cos(q3)+2*m*l*w*sin(q3)-M*r^2-2*m*r^2-M*l^2-M*d^2-2*m*d^2+2*m*l*c-2*m*r*c*cos(-q3+q1+q2)+2*m*r*w*sin(-q3+q1+q2)-4*m*r*d*cos(q1)+4*m*r*l*cos(q1+q2)-2*m*r*c*cos(q1+q2)+2*m*r*w*sin(q1+q2)-2*M*r*d*cos(q1)+2*M*r*l*cos(q1+q2)+4*m*d*l*cos(q2)-2*m*d*c*cos(q2)+2*m*d*w*sin(q2)-2*m*c^2; 
M13 = I+m*r*c*cos(-q3+q1+q2)-m*d*w*sin(-q3+q2)-m*l*w*sin(q3)-m*l*c*cos(q3)-m*r*w*sin(-q3+q1+q2)+m*w^2+m*c^2+m*d*c*cos(-q3+q2); 

M31 = -I+m*l*w*sin(q3)+m*l*c*cos(q3)-m*c^2-m*w^2-m*c*d*cos(-q3+q2)+m*w*d*sin(-q3+q2)-m*c*cos(-q3+q1+q2)*r+m*w*sin(-q3+q1+q2)*r; 
M33 = m*w^2+m*c^2+I; 

RHS1 = -m*g*((d*sin(q1)-l*sin(q1+q2)+c*sin(q1+q2)+w*cos(q1+q2))*cos(gam)-(r+d*cos(q1)-l*cos(q1+q2)+c*cos(q1+q2)-w*sin(q1+q2))*sin(gam))-m*g*((d*sin(q1)-l*sin(q1+q2)+c*sin(-q3+q1+q2)+w*cos(-q3+q1+q2))*cos(gam)-(r+d*cos(q1)-l*cos(q1+q2)+c*cos(-q3+q1+q2)-w*sin(-q3+q1+q2))*sin(gam))-M*g*((d*sin(q1)-l*sin(q1+q2))*cos(gam)-(r+d*cos(q1)-l*cos(q1+q2))*sin(gam))+m*((-d*sin(q1)+l*sin(q1+q2)-c*sin(q1+q2)-w*cos(q1+q2))*(((-d*cos(q1)+l*cos(q1+q2))*u1+l*cos(q1+q2)*u2)*u1+(l*cos(q1+q2)*u1+l*cos(q1+q2)*u2)*u2-(u1+u2)^2*(c*cos(q1+q2)-w*sin(q1+q2)))-(r+d*cos(q1)-l*cos(q1+q2)+c*cos(q1+q2)-w*sin(q1+q2))*(((d*sin(q1)-l*sin(q1+q2))*u1-l*sin(q1+q2)*u2)*u1+(-l*sin(q1+q2)*u1-l*sin(q1+q2)*u2)*u2-(u1+u2)^2*(-c*sin(q1+q2)-w*cos(q1+q2))))+m*((-d*sin(q1)+l*sin(q1+q2)-c*sin(-q3+q1+q2)-w*cos(-q3+q1+q2))*(((-d*cos(q1)+l*cos(q1+q2))*u1+l*cos(q1+q2)*u2)*u1+(l*cos(q1+q2)*u1+l*cos(q1+q2)*u2)*u2-(-u3+u1+u2)^2*(c*cos(-q3+q1+q2)-w*sin(-q3+q1+q2)))-(r+d*cos(q1)-l*cos(q1+q2)+c*cos(-q3+q1+q2)-w*sin(-q3+q1+q2))*(((d*sin(q1)-l*sin(q1+q2))*u1-l*sin(q1+q2)*u2)*u1+(-l*sin(q1+q2)*u1-l*sin(q1+q2)*u2)*u2-(-u3+u1+u2)^2*(-c*sin(-q3+q1+q2)-w*cos(-q3+q1+q2))))+M*((-d*sin(q1)+l*sin(q1+q2))*(((-d*cos(q1)+l*cos(q1+q2))*u1+l*cos(q1+q2)*u2)*u1+(l*cos(q1+q2)*u1+l*cos(q1+q2)*u2)*u2)-(r+d*cos(q1)-l*cos(q1+q2))*(((d*sin(q1)-l*sin(q1+q2))*u1-l*sin(q1+q2)*u2)*u1+(-l*sin(q1+q2)*u1-l*sin(q1+q2)*u2)*u2)); 
RHS3 = -m*g*c*sin(-gam-q3+q1+q2)-m*g*w*cos(-gam-q3+q1+q2)-Th+Thip-m*w*l*u2^2*cos(q3)-2*m*w*u1*l*u2*cos(q3)+m*c*l*u1^2*sin(q3)+m*c*u1^2*d*sin(-q3+q2)-m*w*l*u1^2*cos(q3)+m*c*l*u2^2*sin(q3)+m*w*u1^2*d*cos(-q3+q2)+2*m*c*u1*l*u2*sin(q3); 

MM = [M11 M13; M31 M33];                             

RHS = [RHS1; RHS3];                      

X = MM \ RHS;                                    

ud1 = X(1);                                       
ud2 = 0;                                       
ud3 = X(2);                                       
ud4 = 0;                                        

zdot = [u1 ud1 u2 ud2 u3 ud3 u4 ud4]';

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

function zplus=heelstrike_ss_simplest(t,z,GL_DIM)      

r1 = z(1);   v1 = z(2);                         
r2 = z(3);   v2 = z(4);                         
r3 = z(5);   v3 = z(6);                         
r4 = z(7);   v4 = z(8);                       

q1 = r1 + r2 - r3 - r4;                         
q2 = r4;                                        
q3 = -r3;                                       
q4 = r2; % DEPENDS                           

M = GL_DIM(1);  m = GL_DIM(2); c = GL_DIM(3);   
I = GL_DIM(4);  g = GL_DIM(5); l = GL_DIM(6);   
w = GL_DIM(7);  r = GL_DIM(8); d = GL_DIM(9);   
gam = GL_DIM(10);                                                      

M11 = 2*c^2*m-2*m*l*w*sin(q3)-2*m*d*w*sin(-q3+q2)-2*m*l*c*cos(q3)+M*l^2+2*I+2*w^2*m+2*l^2*m-4*m*d*l*cos(q2)+2*m*d*c*cos(q2)-2*m*d*w*sin(q2)+M*d^2+2*m*d^2+2*m*r^2-2*c*l*m-2*M*d*l*cos(q2)+M*r^2-4*m*r*l*cos(q1+q2)+4*m*r*d*cos(q1)+2*m*r*c*cos(q1+q2)-2*m*r*w*sin(q1+q2)+2*m*r*c*cos(-q3+q1+q2)-2*m*r*w*sin(-q3+q1+q2)-2*M*r*l*cos(q1+q2)+2*M*r*d*cos(q1)+2*m*d*c*cos(-q3+q2); 
M13 = m*d*w*sin(-q3+q2)+m*l*c*cos(q3)+m*l*w*sin(q3)+m*r*w*sin(-q3+q1+q2)-m*r*c*cos(-q3+q1+q2)-m*d*c*cos(-q3+q2)-c^2*m-w^2*m-I; 

M31 = -m*l*w*sin(q3)+m*c^2+m*w^2-m*l*c*cos(q3)+m*c*d*cos(-q3+q2)-m*w*d*sin(-q3+q2)+m*c*cos(-q3+q1+q2)*r-m*w*sin(-q3+q1+q2)*r+I; 
M33 = -m*c^2-m*w^2-I; 

RHS1 = m*((-d*sin(-r3+r1+r2-r4)+l*sin(-r3+r1+r2)-c*sin(-r3+r1+r2)-w*cos(-r3+r1+r2))*((-d*sin(r1)+l*sin(r1+r2))*v1+l*sin(r1+r2)*v2+(-v3+v1+v2)*(-c*sin(-r3+r1+r2)-w*cos(-r3+r1+r2)))-(r+d*cos(-r3+r1+r2-r4)-l*cos(-r3+r1+r2)+c*cos(-r3+r1+r2)-w*sin(-r3+r1+r2))*((l*cos(r1+r2)-d*cos(r1)-r)*v1+l*cos(r1+r2)*v2-(-v3+v1+v2)*(c*cos(-r3+r1+r2)-w*sin(-r3+r1+r2))))+m*((-d*sin(-r3+r1+r2-r4)+l*sin(-r3+r1+r2)-c*sin(r1+r2)-w*cos(r1+r2))*((-d*sin(r1)+l*sin(r1+r2))*v1+l*sin(r1+r2)*v2+(v1+v2)*(-c*sin(r1+r2)-w*cos(r1+r2)))-(r+d*cos(-r3+r1+r2-r4)-l*cos(-r3+r1+r2)+c*cos(r1+r2)-w*sin(r1+r2))*((l*cos(r1+r2)-d*cos(r1)-r)*v1+l*cos(r1+r2)*v2-(v1+v2)*(c*cos(r1+r2)-w*sin(r1+r2))))+M*((-d*sin(-r3+r1+r2-r4)+l*sin(-r3+r1+r2))*((-d*sin(r1)+l*sin(r1+r2))*v1+l*sin(r1+r2)*v2)-(r+d*cos(-r3+r1+r2-r4)-l*cos(-r3+r1+r2))*((l*cos(r1+r2)-d*cos(r1)-r)*v1+l*cos(r1+r2)*v2))+I*(2*v1+2*v2-v3); 
RHS3 = m*c*v1*d*cos(r2)-m*w*v1*d*sin(r2)-m*l*v1*c-m*l*v2*c+m*c^2*v2+m*c^2*v1+m*w^2*v1+m*w^2*v2+m*r*v1*c*cos(r1+r2)-m*r*v1*w*sin(r1+r2)+I*v1+I*v2; 

MM = [M11 M13; M31 M33];                             

RHS = [RHS1; RHS3];                      

X = MM \ RHS;                                    

u1 = X(1);                                       
u2 = 0;                                       
u3 = X(2);                                       
u4 = 0;       %DEPENDS                       

zplus = [q1 u1 q2 u2 q3 u3 q4 u4]; 


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [gstop, isterminal,direction]=collision(t,z,GL_DIM)

M = GL_DIM(1);  m = GL_DIM(2); c = GL_DIM(3);   
I = GL_DIM(4);  g = GL_DIM(5); l = GL_DIM(6);   
w = GL_DIM(7);  r = GL_DIM(8); d = GL_DIM(9);   
gam = GL_DIM(10);   

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

gstop = -l*cos(q1+q2)+d*cos(q1)+l*cos(-q3+q1+q2)-d*cos(q1+q2-q3-q4);
if (q3>-0.05) %no collision detection for foot scuffing
    isterminal = 0;
else
    isterminal=1; %Ode should terminate is conveyed by 1, if you put 0 it goes till the final time u specify
end
direction=-1; % The t_final can be approached by any direction is indicated by this