runjump.m

function runjump
% run the jump simulation

% parameters used by fjump, available to subfunctions
%  Tmax a m g G phidotmax C

% basic dimensions
m = 70; g = 9.81; a = 0.5; % body mass, gravity, half leg-length
Tmax = 2.5*m*g*a;          % max knee torque
T0 = 0.6*Tmax;             % initial knee torque
G = 3;                     % force-velocity shape parameter
phidotmax = 8*sqrt(g/a);   % max shortening velocity 
C = 0.1/(m*g*a);           % knee compliance       
phi0 = 170*pi/180;         % initial knee angle

parms = struct('a', a, 'm', m, 'g', g, 'Tmax', Tmax, ...
    'T0', T0, 'G', G, 'phidotmax', phidotmax, 'C', C, 'phi0', phi0);

% First let's do a standard high jump and a long jump
fprintf(1,'High jump...\n');
phi = phi0;  % knee angle defined as pi for full extension, decreasing in flexion
dist = 2*a*sin(phi0/2); 
theta = 45*pi/180;       % plant angle defined relative to horizontal (bigger is more vertical/steeper)
xdot0 = 3*sqrt(a*g);     % high jump run-up speed
runup = [xdot0, theta];
[height, distance, thigh, statehigh,Fxhigh,Fyhigh] = simjump(runup, parms, 1); % 1 means animate

disp('Press a key to continue');
pause

fprintf(1,'Long jump...\n');
theta = 60*pi/180;       % try a steeper (more vertical) plant angle
xdot0 = 4.5*sqrt(a*g);   % try running up faster for long jump
[height, distance, tlow, statelow, Fxlow, Fylow] = simjump([xdot0, theta], parms, 1);

disp('Press a key to continue');
pause

fprintf(1,'Ground reaction forces vs. time...\n');
clf
subplot(221); plot(thigh,Fyhigh); xlabel('time'); ylabel('Fy'); title('high jump'); set(gca,'xlim',[0 .12],'ylim',[0 12000]);
subplot(222); plot(thigh,Fxhigh); xlabel('time'); ylabel('Fx'); title('high jump'); set(gca,'xlim',[0 .12],'ylim',[0 10000]);
subplot(223); plot(tlow,Fylow); xlabel('time'); ylabel('Fy'); title('long jump'); set(gca,'xlim',[0 .12],'ylim',[0 12000]);
subplot(224); plot(tlow,Fxlow); xlabel('time'); ylabel('Fx'); title('long jump'); set(gca,'xlim',[0 .12],'ylim',[0 10000]);

disp('Press a key to continue');
pause

% loop through initial conditions
fprintf(1,'Vary runup speed and leg plantangle...\n');
thetas = (30:10:80)*pi/180;  % use these values of theta
xdot0s = (1:5)*sqrt(a*g);    % and these values of xdot

for i = 1:length(xdot0s);
	for j = 1:length(thetas);
        xdot0 = xdot0s(i);
		theta = thetas(j);     % (nominally, theta = 45 degrees)
        runup = [xdot0; theta];
        [h(i,j),s(i,j),t,state,Fx,Fy] = simjump(runup, parms);
    
	end;
end;

% make a dimensionless contour plot of heights
clf;
subplot(121)
cs = contour(xdot0s/sqrt(a*g),thetas*180/pi,h'/a,5);
clabel(cs);
xlabel('runup speed'), ylabel('theta'), title('height h');
% find the actual max jump height and indicate it
runup = [3*sqrt(a*g) 55*pi/180];
runupstar = fminsearch(@(x)simjumpnegheight(x,parms), runup);
hold on;
plot(runupstar(1)/sqrt(a*g), runupstar(2)*180/pi, '*');
text(runupstar(1)/sqrt(a*g), runupstar(2)*180/pi, ...
    sprintf('%5.2f',-simjumpnegheight(runupstar,parms)/a));


% make a dimensionless contour plot of distances
subplot(122)
cs = contour(xdot0s/sqrt(a*g),thetas*180/pi,s'/a);
clabel(cs);
xlabel('runup speed'), ylabel('theta'), title('distance s');


% A simulation of the jump with
% no tendon compliance

theta = 45*pi/180; xdot0 = 3*sqrt(a*g); % high jump
theta = 60*pi/180; xdot0 = 4.5*sqrt(a*g); % long jump
runup = [xdot0; theta];
[height, distance, t, state, Fx, Fy] = simjump(runup, parms);
[heightc0, distancec0, tc0, statec0, Fxc0, Fyc0] = simjumpc0(runup, parms);

disp('Heights with and without compliance')
disp([height heightc0]/a)

disp('Distances with and without compliance')
disp([distance distancec0]/a)

% Additional parameter studies
fprintf(1, 'Vary knee angle\n');
phi0s = [165 170 175]*pi/180; 
thetas = (30:10:80)*pi/180;  % use these values of theta
xdot0s = (1:5)*sqrt(a*g);    % and these values of xdot
origparms = parms;
for k = 1:length(phi0s)
    parms.phi0 = phi0s(k);   % change knee angle
    for i = 1:length(xdot0s);
        for j = 1:length(thetas);
        % Set initial conditions
            xdot0 = xdot0s(i);     % (nominally, xdot0 = 3*sqrt(a*g) )
            theta = thetas(j);     
            runup = [xdot0; theta];
            [h(i,j),s(i,j),t,state,Fx,Fy] = simjump(runup, parms);
    	end;
    end;    
    subplot(1,3,k);
    cs = contour(xdot0s/sqrt(a*g),thetas*180/pi,h'/a,5);
    clabel(cs);
    xlabel('runup speed'), ylabel('theta'), 
    title(sprintf('height h (phi = %4.0f°)', phi0s(k)*180/pi));
    % find the actual max jump height and indicate it
    runup = [3*sqrt(a*g) 55*pi/180];
    runupstar = fminsearch(@(x)simjumpnegheight(x,parms), runup);
    hold on;
    plot(runupstar(1)/sqrt(a*g), runupstar(2)*180/pi, '*');
    text(runupstar(1)/sqrt(a*g), runupstar(2)*180/pi, ...
        sprintf('%5.2f',-simjumpnegheight(runupstar,parms)/a));
end

disp('Press a key to continue');
pause

fprintf(1, 'Vary G\n');
clf;
parms = origparms;
Gs = [1 3 5]; 
thetas = (30:10:80)*pi/180;  % use these values of theta
xdot0s = (1:5)*sqrt(a*g);    % and these values of xdot
for k = 1:length(Gs)
    parms.G = Gs(k);   % change knee angle
    for i = 1:length(xdot0s);
        for j = 1:length(thetas);
        % Set initial conditions
            xdot0 = xdot0s(i);     % (nominally, xdot0 = 3*sqrt(a*g) )
            theta = thetas(j);     
            runup = [xdot0; theta];
            [h(i,j),s(i,j),t,state,Fx,Fy] = simjump(runup, parms);
    	end;
    end;    
    subplot(1,3,k);
    cs = contour(xdot0s/sqrt(a*g),thetas*180/pi,h'/a,5);
    clabel(cs);
    xlabel('runup speed'), ylabel('theta'), 
    title(sprintf('height h (G = %3.1f)', Gs(k)));
    % find the actual max jump height and indicate it
    runup = [3*sqrt(a*g) 55*pi/180];
    runupstar = fminsearch(@(x)simjumpnegheight(x,parms), runup);
    hold on;
    plot(runupstar(1)/sqrt(a*g), runupstar(2)*180/pi, '*');
    text(runupstar(1)/sqrt(a*g), runupstar(2)*180/pi, ...
        sprintf('%5.2f',-simjumpnegheight(runupstar,parms)/a));
end
% Lesson: Lower G is better for jump height
% (G governs how sharply torque drops off with velocity)
disp('Press a key to continue');
pause

clf; hold on;
for k = 1: length(Gs);
   parms.G = Gs(k);
   phidots = linspace(-0.2*parms.phidotmax, parms.phidotmax, 20); 
   Ts = torquevelocity(phidots, parms);
   xlabel('Ang vel, phidot'); ylabel('Torque, T');
   title('torque-velocity curves');
   plot(phidots, Ts, 'DisplayName', sprintf('G = %3.1f', Gs(k)));
end
legend

disp('Press a key to continue');
pause

%% ADD CODE HERE TO TEST EFFECT OF VARYING COMPLIANCE CONTINUOUSLY

% end of main runjump code; subfunctions follow

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

function [height,distance,t,state,Fx,Fy] = simjump(runup, parms, anim)
% simulates jump with given initial velocity and angle,
% and returns height and distance of jump
% input is a vector runup = [speed, plantangle]
%   parms parameter structure, and anim = 1 for animatino
% output is height and distance 
%
% parms is a structure containing parameters
%   phi0       initial knee angle (rad)
%   a          leg segment length
%   m          body mass
%   g          gravitational acceleration
%   Tmax       max knee torque
%   T0         initial knee muscle activation
%   G          force-velocity shape parameter
%   phidotmax  max knee ang velocity (force-velocity)
%   C          knee muscle compliance
%   phi0       initial knee angle

%   dist (distance from

if length(runup) < 1   % if speed isn't given, use a default
  xdot0 = 3*sqrt(a*g); % good range is 1:5
else
  xdot0 = runup(1);
end

if length(runup) < 2   % if angle isn't given, use a default
  theta = 45*pi/180;   % good range is 30:80 deg
else
  theta = runup(2);
end

if nargin < 2 % define our own default parameter values
    m = 70; g = 9.81; a = 0.5;
    parms = struct('a', a, ... % half leg-length
      'm', m, ...              % body mass
      'g', g, ...            % gravity
      'Tmax', 2.5*m*g*a, ...      % max knee torque
      'T0', 0.6*(2.5*m*g*a), ...  % initial knee torque
      'G', 3, ...               % force-velocity shape parameter
      'phidotmax', 8*sqrt(g/a), ...          % max velocity 
      'C', 0.1/m*g*a, ...      % knee compliance
      'phi0', 170*pi/180);       % initial knee angle 
end

a = parms.a; m = parms.m; g = parms.g;
Tmax = parms.Tmax; T0 = parms.T0; G = parms.G;
phidotmax = parms.phidotmax; C = parms.C;
phi0 = parms.phi0;
 
% Set initial conditions
phi = phi0;      
dist = 2*a*sin(phi/2);
T0 = Tmax*0.6;         % start w/ muscle activated

x0 = -cos(theta)*dist; % initial x, y positions
y0 =  sin(theta)*dist;
ydot0 = 0;             % body moves horizontally initially

% state vector 
xstart = [x0; y0; xdot0; ydot0; T0];
% integrate the ode in fjump for up to 0.3 seconds, stopping at ground
% contact
options = odeset('events', @jumpevent);
odesol = ode45(@fjump, [0 0.3], xstart, options);

t = odesol.x'; state = odesol.y';
jumptime = odesol.x(end);
x = odesol.y(1,:)';
y = odesol.y(2,:)';
xdot = odesol.y(3,:)';
ydot = odesol.y(4,:)';
T = odesol.y(5,:)';
dist = sqrt(x.^2 + y.^2);     % dist from foot to hip
theta = atan2(y, -x);
phiover2 = asin(dist/(2*a));
if cos(phiover2) ~= 0,        % calculate ground force
  F = T./(a*cos(phiover2)); 
else
  F = 0;
end;
Fx = F.*cos(theta);
Fy = F.*sin(theta);
xoff = odesol.y(1,end)';       % final conditions of ground
yoff = odesol.y(2,end)';       % contact phase
xdotoff = odesol.y(3,end)';
ydotoff = odesol.y(4,end)';
airtime = 1/g * (ydotoff + sqrt(ydotoff^2 + 2*g*yoff));
height = yoff + ydotoff^2/(2*g);  % height
distance = xdotoff * airtime;     % distance traveled

% animation settings
if nargin == 3 & anim == 1 % animate it
    dT = 0.01; runtime = 0.1; % runtime is how much run-up to show
    trun = -fliplr((dT:dT:runtime))'; % run-up is in negative time
    tanim = [trun; (0:dT:jumptime+airtime)'];
    tjump = (0:dT:jumptime)';
    taerial = tanim(tanim > jumptime);

    xrun = xdot0*trun + x0; yrun = xrun*0 + y0;
    states = deval(odesol, tjump)';
    xjump = states(:,1);
    yjump = states(:,2);

    xaerial = xoff + xdotoff*(taerial-jumptime);
    yaerial = yoff + ydotoff*(taerial-jumptime) - 1/2*g*(taerial-jumptime).^2;

    xs = [xrun; xjump; xaerial]; ys = [yrun; yjump; yaerial];
    xlimit = [min(xs)-a/2 max(xs)+a/2]; ylimit = [-0.05 max(ys)+a*1.5];

    clf
    set(gcf, 'color', [1 1 1]); set(gca,'DataAspectRatio',[1 1 1],'Visible','off',...
      'NextPlot','Add','xlim',xlimit,'ylim',ylimit);

    thetas = [0:.2:2*pi 0]; Q = diag([12/a 3/a]);
    r = 1 ./sqrt(Q(1,1)*cos(thetas).^2 + 2*Q(1,2)*cos(thetas).*sin(thetas) + ...
      Q(2,2)*sin(thetas).^2);
    xbody = r.*cos(thetas); ybody = r.*sin(thetas)+a/2;
    hbody = line(xbody, ybody+y0);
    th0 = atan2(y0, -x0); phiover20 = asin(sqrt(x0^2+y0^2)/(2*a));
    xleg = [0 a*sin(pi-th0-phiover20) 2*a*cos(pi/2-th0)];
    yleg = [0 -a*cos(pi-th0-phiover20) -2*a*sin(pi/2-th0)];
    hleg = line(xleg, yleg+y0);
    hgnd = line(xlimit,[0 0],'color',[0 0 0],'linewidth',2);

    for i=1:length(xs)
      set(hbody,'xdata',xbody+xs(i),'ydata',ybody+ys(i));
      set(hleg,'xdata',xleg+xs(i));
      if tanim(i) >= 0
        if tanim(i) <= tjump
          th = atan2(ys(i),-xs(i)); phiover2 = asin(sqrt(xs(i)^2+ys(i)^2)/(2*a));
          xleg = [0 a*sin(pi-th-phiover2) 2*a*cos(pi/2-th)];
          yleg = [0 -a*cos(pi-th-phiover2) -2*a*sin(pi/2-th)];
          set(hleg,'xdata',xleg+xs(i),'ydata',yleg+ys(i));
        else
          set(hleg,'visible','off'); 
        end
      end
      pause(0.01)
    end
end % animation

end % simjump

function negh = simjumpnegheight(runup,parms)
% returns negative of jump height, so that minimizing this function
% will maximize jump height
    [h,d] = simjump(runup,parms);
    negh = -h;
end % simjumpheight

function negd = simjumpnegdistance(runup,parms)
% returns negative of jump distance, so that minimizing this function
% will maximize jump distance
    [h,d] = simjump(runup,parms);
    negd = -d
end % simjumpdistance

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

function [height,distance,t,state,Fx,Fy] = simjumpc0(runup, parms)
% simulates jump with given initial velocity and angle,
% and returns height and distance of jump
% input is a vector runup = [speed, plantangle]
%   parms parameter structure, and anim = 1 for animatino
% output is height and distance 
%
% parms is a structure containing parameters
%   phi0       initial knee angle (rad)
%   a          leg segment length
%   m          body mass
%   g          gravitational acceleration
%   Tmax       max knee torque
%   T0         initial knee muscle activation
%   G          force-velocity shape parameter
%   phidotmax  max knee ang velocity (force-velocity)
%   phi0       initial knee angle
%
% This version is for jump with no tendon compliance

if length(runup) < 1   % if speed isn't given, use a default
  xdot0 = 3*sqrt(a*g); % good range is 1:5
else
  xdot0 = runup(1);
end

if length(runup) < 2   % if angle isn't given, use a default
  theta = 45*pi/180;   % good range is 30:80 deg
else
  theta = runup(2);
end

if nargin < 2 % define our own default parameter values
    m = 70; g = 9.81; a = 0.5;
    parms = struct('a', a, ... % half leg-length
      'm', m, ...              % body mass
      'g', g, ...            % gravity
      'Tmax', 2.5*m*g*a, ...      % max knee torque
      'T0', 0.6*(2.5*m*g*a), ...  % initial knee torque
      'G', 3, ...               % force-velocity shape parameter
      'phidotmax', 8*sqrt(g/a), ...          % max velocity 
      'phi0', 170*pi/180);       % initial knee angle 
end

a = parms.a; m = parms.m; g = parms.g;
Tmax = parms.Tmax; T0 = parms.T0; G = parms.G;
phidotmax = parms.phidotmax; 
phi0 = parms.phi0;
 
% Set initial conditions
phi = phi0;      
dist = 2*a*sin(phi/2);
x0 = -cos(theta)*dist; % initial x, y positions
y0 =  sin(theta)*dist;
ydot0 = 0;             % body moves horizontally initially

xstart = [x0; y0; xdot0; ydot0];

options = odeset('events', @jumpeventc0); % no compliance event
odesol = ode45(@fjumpc0, [0 .2], xstart(1:4), options); % no compliance state-derivative
t = odesol.x'; state = odesol.y';
x = odesol.y(1,:)';
y = odesol.y(2,:)';
xdot = odesol.y(3,:)';
ydot = odesol.y(4,:)';
dist = sqrt(x.^2 + y.^2);
ddot = (x.*xdot + y.*ydot) ./dist;
theta = atan(-y ./ x);
phiover2 = asin(dist/(2*a));
phidot = ddot ./ (a*cos(phiover2));
T = Tmax*((phidotmax - phidot)./(phidotmax+G*phidot));
T(phidot <= 0) = Tmax*ones(sum(phidot<=0),1);
F = T./(a*cos(phiover2));
Fx = F.*cos(theta);
Fy = F.*sin(theta);
xoff = state(end,1);       % final conditions of ground
yoff = state(end,2);       % contact phase
xdotoff = state(end,3);
ydotoff = state(end,4);
height = yoff + ydotoff^2/(2*g);  % height
tair = 1/g * (ydotoff + sqrt(ydotoff^2 + 2*g*yoff));
distance = xdotoff * tair;          % distance traveled

end % simjumpc0

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

function xdot = fjump(t, x);
% xdot = fjump(t,x) returns the state-derivative for the
% high and long-jump simulation, where the state x contains
% [x; y; xdot; ydot; T] where x and y are the position of
% the center of mass, xdot and ydot are velocities, and T
% is the torque.  

% The following parameters should be defined in the workspace:
%   Tmax a m g G phidotmax C

xc = x(1);     % x position of center of mass
yc = x(2);     % y position of center of mass
xcdot = x(3);  % x velocity
ycdot = x(4);  % y velocity
T = x(5);      % torque T

dist = sqrt(xc^2 + yc^2);
ddot = (xc*xcdot + yc*ycdot)/dist;  % derivative of dist
phiover2 = asin(dist / (2*a));
theta = atan2(yc, -xc);
phidot = ddot / (a*cos(phiover2));
phidotc = -phidotmax * (T/Tmax - 1)/(G*T/Tmax + 1);
Tdot = -(phidot - phidotc)/C;
if (T >= Tmax) & (Tdot > 0),   % torque can't go past Tmax
  Tdot = 0;
end;
if T < 0,                      % nor can torque go below zero
  Tdot = -5000*T;
end;
F = T / (a*cos(phiover2));     % test for leg fully extended
if imag(phiover2) ~= 0 | (phiover2 > pi/2),
  F = 0;
  Tdot = 0;
end;
xcdotdot = -(F/m)*cos(theta);
ycdotdot = (F/m)*sin(theta) - g;

xdot = [xcdot; ycdot; xcdotdot; ycdotdot; Tdot];

end % fjump

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

function xdot = fjumpc0(t, x);
% xdot = fjumpc0(t,x) returns the state-derivative for the
% high and long-jump simulation, where the state x contains
% [x; y; xdot; ydot; T] where x and y are the position of
% the center of mass, xdot and ydot are velocities, and T
% is the torque.  The following global variables must be
% defined to pass parameters to fjump: Tmax a m g G phidotmax C
% fjumpc0 assumes that the tendon has zero compliance

% The following parameters should be defined in the workspace:
%   Tmax a m g G phidotmax

xc = x(1);
yc = x(2);
xcdot = x(3);
ycdot = x(4);

dist = sqrt(xc^2 + yc^2);
ddot = (xc*xcdot + yc*ycdot)/dist;
phiover2 = asin(dist / (2*a));

theta = atan2(real(yc), -real(xc));
phidot = ddot / (a*cos(phiover2));
T = Tmax*((phidotmax - phidot)/(phidotmax+G*phidot));
if (phidot) <= 0,
	T = Tmax;
end;
F = T / (a*cos(phiover2));

xcdotdot = -(F/m)*cos(theta);
ycdotdot = (F/m)*sin(theta) - g;

xdot = [xcdot; ycdot; xcdotdot; ycdotdot];

end % fjumpc0

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

function [event, isterm, dir] = jumpevent(t, x);
% Event function that goes with fjump

% The following parameters should be defined in the workspace:
%   Tmax a m g G phidotmax C

xc = x(1);     % x position of center of mass
yc = x(2);     % y position of center of mass
xcdot = x(3);  % x velocity
ycdot = x(4);  % y velocity
T = x(5);      % torque T

event = [T; 4*a*a-xc^2+yc^2];   % check for when T goes to zero or leg gets fully extended
isterm = [1; 1];   % stop when it does
dir = [0; 0];   % regardless of slope

end % jumpevent

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

function [event, isterm, dir] = jumpeventc0(t, x);
% Event function that goes with fjumpc0

% The following parameters should be defined in the workspace:
%   Tmax a m g G phidotmax C

xc = x(1);
yc = x(2);
xcdot = x(3);
ycdot = x(4);

dist = sqrt(xc^2 + yc^2);
ddot = (xc*xcdot + yc*ycdot)/dist;
phiover2 = asin(dist / (2*a));

theta = atan2(yc, -xc);
phidot = ddot / (a*cos(phiover2));
T = Tmax*((phidotmax - phidot)/(phidotmax+G*phidot));
if (phidot) <= 0,
	T = Tmax;
end;

event = T;   % check for when T goes to zero
isterm = 1;   % stop when it does
dir = 0;   % regardless of slope

end % jumpeventc0

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

function T = torquevelocity(phidot, parms)
    phidotmax = parms.phidotmax;
    Tmax = parms.Tmax;
    G= parms.G;
    
    T = Tmax*((phidotmax - phidot)./(phidotmax+G*phidot));
    T(phidot <= 0) = Tmax*ones(sum(phidot<=0),1); 
end % torquevelocity

end % runjump file