function [yout]=int_3D_step_mcg(t0,tfinal,y0,tol);
% Does forward integration ONE STEP ONLY for a 3D walker with
% 4 DOF. Assumes that relevant init file has been called.
% This file computes the "stride function", the next
% step ICs as functions of the previous ones.
global g gam m p34 pc3 pc4 p4f Icm3 Icm4 hspr hdamp sspr sdamp
% don't put global h up here, since a different h is used in here!
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% C.B. Moler, 3-25-87, 10-5-91, 6-3-93.
% Copyright (c) 1984-93 by The MathWorks, Inc.
% Modified for walking simulations
% by Mariano Garcia (garcia@tam.cornell.edu), 1998
%%%%%%%%%%%%%%%%%%%%%%%%%% Program begins %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% See SUBTLE POINT, below
% Initialization
% The Fehlberg coefficients:
beta = [ [ 1 0 0 0 0 0]/4
[ 3 9 0 0 0 0]/32
[ 1932 -7200 7296 0 0 0]/2197
[ 8341 -32832 29440 -845 0 0]/4104
[-6080 41040 -28352 9295 -5643 0]/20520 ]';
gamma = [ [902880 0 3953664 3855735 -1371249 277020]/7618050
[ -2090 0 22528 21970 -15048 -27360]/752400 ]';
pow = 1/5;
f = zeros(length(y0),6);
t = t0; % if this is ever not zero, I will eat my proverbial hat
k=0; % some remnant from defining the mapping to be 2 steps instead of 1
% This isn't the usual k from ODE(x)(x+1). I don't store trajectories with
% this integrator file.
hmax = (tfinal - t)/16;
h = hmax/8;
y = y0(:);
tau = tol * max(norm(y, 'inf'), 1);
clf reset;
% hf1 is height of swing foot above ramp surface
% hfdot1 is the vertical velocity of the swing foot
hf1=0;
hfdot1=1;
%%%%%%%%%%%%%%%%%%%%% Main Loop Begins %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% See SUBTLE POINT, below
while k<1
while (hfdot1>0) & k < 1 % swing foot on the way up
% Compute the slopes
junk = yderivs_3D(t,y);
f(:,1) = junk(1:8);
hf1=junk(9);
hfdot1 = junk(10);
for j = 1:5
junk = yderivs_3D(t,y+h*f*beta(:,j));
f(:,j+1) = junk(1:8);
end % j = 1:5 loop
% Estimate the error and the acceptable error
delta = norm(h*f*gamma(:,2),'inf');
tau = tol*max(norm(y,'inf'),1.0);
% Update the solution only if the error is acceptable
if delta <= tau
t = t + h;
y = y + h*f*gamma(:,1);
end; % delta <= tau loop
% Update the step size
if delta ~= 0.0
h = min(hmax, 0.8*h*(tau/delta)^pow);
end
end; % (hfdot1>0) & k < 1 loop
% SUBTLE POINT: to duplicate McGeer's fixed point, one needs to let the
% swing foot pass underground slightly. The fixed points that are in
% init_tinkpar1.m also need this allowance. The more up-to-date fixed
% points that are on my personal PC (in transit to CA) do not need this
% allowance and will not work with this integrator as is.
% The following loops allow for this passing underground.
% If this is not desired, get rid of the first two if-then loops.
% I have another version of this integrator with these two
% loops removed.
while ((hf1+h*hfdot1<0) | hfdot1 < 0) & k < 1 % swing foot on the
% way down or below ground
% Sometimes this integrator will fail because the swing foot never comes
% back above the ground. Have to watch out for that.
% Compute the slopes
junk = yderivs_3D(t,y);
f(:,1) = junk(1:8);
hf1=junk(9);
hfdot1 = junk(10);
for j = 1:5
junk = yderivs_3D(t,y+h*f*beta(:,j));
f(:,j+1) = junk(1:8);
end % j = 1:5 loop
% Estimate the error and the acceptable error
delta = norm(h*f*gamma(:,2),'inf');
tau = tol*max(norm(y,'inf'),1.0);
% Update the solution only if the error is acceptable
if delta <= tau
t = t + h;
y = y + h*f*gamma(:,1);
end; % delta <= tau loop
% Update the step size
if delta ~= 0.0
h = min(hmax, 0.8*h*(tau/delta)^pow);
end % delta ~= 0.0 loop
end; % ((hf1+h*hfdot1<0) | hfdot1 < 0) & k < 1 loop
while (hf1+h*hfdot1>0) & k<1 % until swing foot hits ground
% Compute the slopes
junk = yderivs_3D(t,y);
f(:,1) = junk(1:8);
hf1=junk(9);
hfdot1 = junk(10);
for j = 1:5
junk = yderivs_3D(t,y+h*f*beta(:,j));
f(:,j+1) = junk(1:8);
end % j = 1:5 loop
% Estimate the error and the acceptable error
delta = norm(h*f*gamma(:,2),'inf');
tau = tol*max(norm(y,'inf'),1.0);
% Update the solution only if the error is acceptable
if delta <= tau
t = t + h;
y = y + h*f*gamma(:,1);
end; % delta <= tau loop
% get new hf1, hf1dot so we know them exactly
junk = yderivs_3D(t,y);
hf1=junk(9);
hfdot1 = junk(10);
% Update the step size
if delta ~= 0.0
h = min(hmax, 0.8*h*(tau/delta)^pow);
end % delta ~= 0.0 loop
end; % (hf1+h*hfdot1>0) & k<1 loop
horig=h; % store the original time step because we are about to make
% it really small in the next loop.
%%%%%%%%%%%%%% Nearing Heelstrike %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Now we are less than h `seconds' away from heelstrike.
% Do Newton's method to get to kneestrike
% or Poincare's method or whatever it is.
% Assume we want to find the right h to get us right to heelstrike
% swing foot is at height junk(9) moving with vertical
% velocity junk(10). Calculate h and iterate to convergence.
while abs(hf1) > tol/1000 & k<1 % do this loop until foot
% height is much less than tol.
h=(-hf1/hfdot1); % predicted h
% Compute the slopes
junk = yderivs_3D(t,y);
f(:,1) = junk(1:8);
for j = 1:5
junk = yderivs_3D(t,y+h*f*beta(:,j));
f(:,j+1) = junk(1:8);
end % j = 1:5 loop
% Assume error is aceptable since h is small, update the solution
t = t + h;
y = y + h*f*gamma(:,1);
% get new hf1, hf1dot so we know what it is precisely
junk = yderivs_3D(t,y);
hf1=junk(9);
hfdot1=junk(10);
end; % abs(hf1) > (tol/1000) & t<tfinal loop
% Now we are exactly at the pre-heelstrike state
%%%%%%%%%%%%%%%%%%%%%%%%%%%% Heelstrike %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if k<1
ynew = heelstrike_mcg(y); % call the heelstrike routine to calculate
% new angular rates and swap L-R parameters.
hf1 = 0; % reset foot height
hfdot1=1; % reset foot velocity to some bogus temporary thing
y = ynew'; % set the state variable to whatever ynew is
k=k+1; % increment the step number, if you are using this
h=horig/3; % reset time step h to something reasonable
end; % k<1 loop
%%%%%%%%%%%%%%%%%%%%%%%%% End Main Loop %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end; % main k<1 loop
yout=y; % return the value of the state at the beginning of next step
