%Simplest Walker %Last Modified 3/18/2009 % Josh Petersen % Based on rimlesswheel.m by Pranav Bhounsule function simplest_walker clc clear all close all format long M = 1000; m = 1.0; I = 0.00; l = 1.0; w = 0.0; c = 1.0; r = 0.0; d = 0.00; g = 1.0; gam = 0.009; 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 %%%% Stability, using linearised eigenvalue %%% J=partialder(@onestep,zstar,GL_DIM); disp('EigenValues for linearized map are'); eig(J) [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)); xa2=xh+l*sin(q3(i)-q1(i)-q2(i)); ya2=yh+l*cos(q3(i)-q1(i)-q2(i)); figure(1) plot([xh xa1],[yh ya1]);%plots the right half of the walker hold on; plot([xh xa2],[yh ya2]);% plots the left half plot([xh-1 xh+1],[0 0],'black')%plots the ground line %plot(xh,yh,'ko-','LineWidth',3) axis([xh-1 xh+1 -1 1.5]); axis off; hold off; end figure(2) hold on plot(t,q1,'r'); plot(t,q3,'b'); xlabel('Time (s)'); ylabel('Angle (rad)'); legend('Stance Angle','Swing Angle') hold off %%%%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); 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 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=0; 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