runanthrowalk2.m

function runanthrowalk2(defaultparms)
% RUNANTHROWALK2    Anthropomorphic walking model simulation for one step
%
%   xnext = runanthrowalk2([parms])
% 
% Perform a study of the anthropomorphic walking model in 2D.
% Demonstrations include:
%   1. one step simulation
%   2. determination of periodic gait (fixed point)
%   3. animation
%   4. testing of energy conservation
%   5. parameter study R using continuation method


% State vector:
% x = [q1; q2; q1dot; q2dot], angles of stance and
% swing legs, and respective angular velocities.
% All angles are measured counter-clockwise from vertical.

% Default model parameters 
% Base units:
% M = total body mass, L = leg length, g = gravitational acceleration
%   (These are set to unity, so that the model is effectively dimensioned
%    by M, L, g as base units. All other units are relative to these.)

% Anthropomorphic model parameters (after McGeer, 1990):
%   gamma = downward slope (rad), Mp = pelvis mass, Ml = leg mass, 
%   Ip = pelvis moment of inertia, Il = leg moment of inertia, 
%   C = distance from bottom of leg segment to center of mass
%   R = radius of foot arc

% Simulation parameters in parms.sim structure
%   tmax = 5 (max runtime), ntimesteps = 0 for variable time steps,
%                                      = N for N equally spaced steps.

x0 = [0.3 -0.3 -0.3 -0.25]'; 
% state vector: x = [q1; q2; q1dot; q2dot]
    
if nargin < 1 % set default parameters 
    defaultparms = struct('M', 1, 'L', 1, 'g', 1, ...
        'gamma', 0.016, 'Mp', 0.68, 'Ml', 0.16, ...
        'Ip', 0, 'Il', 0.017, 'C', 0.645, 'R', 0.3, ...
        'sim', struct('tmax', 5, 'ntimesteps', 18));
    % where Il = Ml*rgyr^2, rgyr = 0.326 radius of gyration of the leg
end % otherwise parms may be specified as argument

parms = defaultparms;

% Set parameters values
M = parms.M; L = parms.L; g = parms.g;
gamma = parms.gamma; Mp = parms.Mp; Ml = parms.Ml;
Ip = parms.Ip; Il = parms.Il; C = parms.C; R = parms.R;


%% One step simulation
[xe,te,ts,xs] = onestepanthrowalk2(x0, parms);
disp('Initial condition for next step:')
disp(xe')

disp('Ending time of step:')
disp(te)

plot(ts, xs) % times and states over a full step
xlabel('Time (dimensionless)');
ylabel('States'); 
legend('q1','q2','q1dot','q2dot');

%% Find a periodic gait
% Define an error function, the root of which will denote a fixed point
% (state corresponding to periodic gait or limit cycle)
fixedpointerror = @(x) onestepanthrowalk2(x, parms) - x;

% Next search for the actual fixed point
% Search for the initial condition
xstar = findroot(fixedpointerror, x0);
disp('Here is the error in fixed point:');
disp(fixedpointerror(xstar)') 

% Now find the Jacobian of the return map, to evaluate stability
J = fjacobian(@(x) onestepanthrowalk2(x,parms), xstar);
[v,d] = eig(J);   % v contains eigenvectors, d contains eigenvalues (floquet multipliers)   
disp('Step-to-step eigenvalues');
disp(diag(d))

disp('Step-to-step eigenvectors');
disp(v)

%% Animate the step
% Compute an entire step at equal time steps and store the results:
parms.sim.ntimesteps = 18; % set # of time steps as simulation parameter

% Use the periodic gait to run one step simulation
[xnext, tcontact, ts, xs] = onestepanthrowalk2(0, xstar, parms);  

clf; animateanthrowalk2(xs,2, parms) % Animate the step for two steps

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

%% Test for energy conservation
% Compute the total energy and verify that it is conserved over time
% check for energy conservation during the simulation
[energies, KEs, PEs] = energyanthrowalk2(xs, parms); % for all states over time

clf
subplot(121);
plot(ts, xs(:,1:2)); xlabel('Time'); ylabel('Angles'); legend('q1', 'q2');
subplot(122);
plot(ts, energies, ts, KEs, ts, PEs); xlabel('Time'); ylabel('Energy');
legend('Total', 'KE','PE');
title('Energy conservation')

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

%% Parameter study 1
% Perform a parameter study on radius of curvature R:
% A parameter is varied over many values, and the corresponding gait
% is found for each value.  Then certain variables (e.g., speed, eigenvalues)
% can be computed for each parameter value.

parms = defaultparms; % reset parameter values
Rs = 0.3:-0.02:0; % look at radius of curvature from 0.3 to 0:
fixedpointerror = @(x) onestepanthrowalk2(x, parms) - x;

x0 = xstar; % use the previously found gait as an initial guess
Nstates = 4; % number of system states
eigs = zeros(length(Rs),Nstates);
steplens = zeros(length(Rs),1);
steptimes = zeros(length(Rs),1);
speeds = zeros(length(Rs),1);
for j = 1:length(Rs)
  R = Rs(j);
  parms.R = R;  % set the R field
  % find the fixed point for this parameter value
  fixedpointerror = @(x) onestepanthrowalk2(x, parms) - x;
  x0 = findroot(fixedpointerror, x0);
  % compute speed
  [xnext,tcontact,t,x] = onestepanthrowalk2(x0, parms);
  % step time is tcontact:
  J = fjacobian(@(x) onestepanthrowalk2(x,parms), x0);
  eigs(j,:) = eig(J)'; % store the eigenvalues here
  steplens(j) = (2*(L-R)*sin(x0(1))+ 2*R*x0(1)); % how to find step length
  steptimes(j) = tcontact;
  speeds(j) = steplens(j)/tcontact;
end
% Here is an example of a plot that can be made from the results
clf; 
subplot(121);
plot(Rs, speeds); xlabel('Foot radius of curvature'); ylabel('Speed');
title('Varying foot radius of curvature');
subplot(122);
plot(Rs, steplens, Rs, steptimes); xlabel('Foot radius of curvature'); 
legend('Step length','Step time');

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



%% End of anthrowalk2; local functions follow

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xnext,tcontact,ts,xs] = onestepanthrowalk2(t0, x0, optparms)
% [xnext,tcontact,ts,xs] = onestepanthrowalk2([t0, ] x0 [, parms])
% onestepanthrowalk2 performs one step of the planar dynamic walking
% model, whose equations of motion are derived in 
% Planar2leg.nb.  Inputs are initial state x0.
% returns xnext (initial state of next step), tcontact, 
% and arrays ts and xs containing steps from integration.
% If you intend to animate the simulation, include the
% simulation paramter parms.sim.ntimesteps to the number of
% equally spaced frames desired.
%
% Optionally can be called with an initial time,
%   onestepanthrowalk2(t0, x0)
% Optionally can be called with parameters structure
%   onestepanthrowalk2([t0,], x0, parms)
% where parms should either be in enclosing scope, or final argument.

% Simulation parameters in enclosing scope:
%   parms.sim.tmax parms.sim.ntimesteps

if nargin == 1 % if only one argument is given, assume it is x0
  x0 = t0; t0 = 0;
elseif nargin == 2 % need to figure out whether second argument is parms
  if isstruct(x0)
    parms = x0; % input was x0, parms
    x0 = t0; t0 = 0; 
  else          % input was t0, x0 so parms should be in scope
    if ~exist('parms',1) % doesn't exist
      error('No parms struct in scope');
    end
  end
elseif nargin == 3   % explicitly fed parms struct
  parms = optparms;
end

% these statements are necessary in order to set event handling:
% ode45 will stop the integration when the event occurs
options = odeset('events', @eventanthrowalk2);

% integrate using ode45 and the state-derivative function
odesol = ode45(@(t,x) fanthrowalk2(t,x,parms), [t0 t0+parms.sim.tmax], x0, options);

ts = odesol.x'; % time vector (as a column)
xs = odesol.y'; % states array (states as rows, time as column);

tcontact = odesol.xe;
xe = odesol.ye;

if isempty(xe)
    warning('no event detected by ode45');
end

xminus = xe;  % event-detected state

% Computes initial state for next step:
xnext = s2santhrowalk2(xminus, parms);

if nargout == 0  % if no output, plot just the angles
  plot(ts, xs(:,1:2))
end

% check for equal time steps, e.g. for animation
if parms.sim.ntimesteps ~= 0 % any non-zero value means equal time steps
    ts = linspace(ts(1), ts(end), parms.sim.ntimesteps);
    xs = deval(odesol, ts)'; % use the solution structure to evaluate
                            % states at arbitrary time points (deval)
end

end % onestepanthrowalk2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xdot = fanthrowalk2(t, x, optparms)
% state-derivative function for anthropomorphic dynamic walking model
% state is [q1; q2; q1dot; q2dot]
% calculates state derivative for two-segment, 2-D dynamic
% walker.

if nargin == 3 % parameters fed in as third argument
    parms = optparms;
end            % otherwise expect parms to be in scope

% Parameters: M L g gamma Mp Ml Ip Il C R
M = parms.M; L = parms.L; g = parms.g;
gamma = parms.gamma; Mp = parms.Mp; Ml = parms.Ml;
Ip = parms.Ip; Il = parms.Il; C = parms.C; R = parms.R;

sg = sin(gamma);
cg = cos(gamma);

q1 = x(1); q2 = x(2); % measured counter-clockwise from vertical
u1 = x(3); u2 = x(4); % respective q1dot, q2dot

c1 = cos(q1); c2 = cos(q2); c12 = cos(q1-q2);
s1 = sin(q1); s2 = sin(q2); s12 = sin(q1-q2);

% equations of motion are of the form MM*udot = rhs
% where MM = mass matrix, rhs = right-hand-side, udot = u1dot, u2dot

MM = zeros(2,2); rhs = zeros(2,1);

MM(1,1) = Il - 2*C*Ml*R + 2*C*c1*Ml*R - 2*L*Ml*R + 2*c1*L*Ml*R - 2*L*Mp*R + 2*c1*L*Mp*R + Ml*(C*C) + ...
   Ml*(L*L) + Mp*(L*L) + 4*Ml*(R*R) - 4*c1*Ml*(R*R) + 2*Mp*(R*R) - 2*c1*Mp*(R*R);

MM(1,2) = C*c12*L*Ml - C*c12*Ml*R + C*c2*Ml*R + c12*L*Ml*R - c2*L*Ml*R - ...
   c12*Ml*(L*L);
MM(2,1) = MM(1,2);

MM(2,2) = Il - 2*C*L*Ml + Ml*(C*C) + Ml*(L*L);

rhs(1) = -(cg*g*Ml*(-C + R)*s1) - cg*g*Ml*(-L + R)*s1 - ...
   cg*g*Mp*(-L + R)*s1 + (-(g*Ml*R) + c1*g*Ml*(-C + R))*sg + ...
   (-(g*Ml*R) + c1*g*Ml*(-L + R))*sg + ...
   (-(g*Mp*R) + c1*g*Mp*(-L + R))*sg + ...
   s1*(C*Ml*R + L*Ml*R + L*Mp*R - 2*Ml*(R*R) - Mp*(R*R))*(u1*u1) + ...
   ((C*Ml*R - L*Ml*R)*s2 + s12* ...
       (-(C*L*Ml) + C*Ml*R - L*Ml*R + Ml*(L*L)))*(u2*u2);

rhs(2) = s12*(C*L*Ml - C*Ml*R + L*Ml*R - Ml*(L*L))*(u1*u1) + ...
    %% FILL IN THE MISSING TERMS HERE

udot = MM\rhs;

xdot = [x(3); x(4); udot];

end % fanthrowalk2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [value, isterminal, direction] = eventanthrowalk2(t, x, optparms)
% returns event function for anthropomorphic walking simulation

% Here is how event checking works:  
% At each integration step, ode45 checks to see if an
% event function passes through zero (in this case, we need
% the function to go through zero when the foot hits the
% ground).  It finds the value of the event function by calling
% eventrw, which is responsible for returning the value of the 
% event function in variable value.  isterminal should contain
% a 1 to signify that the integration should stop (otherwise it
% will keep going after value goes through zero).  Finally,
% direction should specify whether to look for event function
% going through zero with positive or negative slope, or either.

if nargin == 3 % parameters fed in as third argument
    parms = optparms;
end            % otherwise expect parms to be in scope

% we want to stop the simulation when foot touches ground
q1 = x(1); q2 = x(2); u1 = x(3); u2 = x(4);

value = cos(q1) - cos(q2); % foot height (ignoring L); alternatively use q1 - q2

% Here is a trick to use to ignore heel scuffing, by 
% making sure the swing leg is coming backwards when
% it hits the ground, and the stance is past halfway.
% This effectively only allows "long-period" gaits.
value = (cos(q1) - cos(q2))*(u2 < 0)*(q1 < 0);
isterminal = 1;  % tells ode45 to stop when event occurs
direction = -1;  % tells ode45 to look for negative crossing

end % eventanthrowalk2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function xnew = s2santhrowalk2(xminus, optparms)
% Calculates the new state following foot contact, for anthropomorphic
% walking model.
% Angular momentum is conserved about the impact point for the
% whole machine, and about the hip joint for the trailing leg.
% After conservation of angular momentum is applied, the legs
% are switched.
% State vector: qstance, qswing, qdotstance, qdotswing

if nargin == 3 % parameters fed in as third argument
    parms = optparms;
end            % otherwise expect parms to be in scope

% Parameters: M L g gamma Mp Ml Ip Il C R
M = parms.M; L = parms.L; g = parms.g;
gamma = parms.gamma; Mp = parms.Mp; Ml = parms.Ml;
Ip = parms.Ip; Il = parms.Il; C = parms.C; R = parms.R;

sg = sin(gamma); cg = cos(gamma);

MM = zeros(2,2);
amb = zeros(2,1);

q1 = xminus(1); q2 = xminus(2); u1 = xminus(3); u2 = xminus(4);

c1 = cos(q1); c2 = cos(q2); c12 = cos(q1-q2);
s1 = sin(q1); s2 = sin(q2); s12 = sin(q1-q2);

% Angular momentum before impact:
%   amb(1) is angular momentum of whole system about heel contact
%   amb(2) is angular momentum of trailing leg about the hip

amb(1) = Il*u1 + Ml*R*(c2*(C - R) + R)*u1 + Mp*R*(c2*(L - R) + R)*u1 + ...
   Ml*R*(c1*(C - L) + c2*(L - R) + R)*u1 + Ml*(C - R)*(C - L + c12*(L - R) + c1*R)*u1 - ...
   (c12*Ml*(C - R) + c1*Ml*R)*(-((-C + L)*u1) - (C - R)*u1) - ...
   (c12*Mp*(L - R) + c1*Mp*R)*(-((-C + L)*u1) - (C - R)*u1) + Il*u2 + ...
   (C - L)*Ml*(C - R + c2*R)*u2;

amb(2) = (Il + (C - L)*Ml*(C - R))*u1 + c1*(C - L)*Ml*R*u1;

% Angular momentum after heel strike:
%   We are going to switch the names of the legs simultaneously with
%   computing the impact, so the cosines and sines are switched:
c1 = cos(q2); c2 = cos(q1); s1 = sin(q2); s2 = sin(q1);

%   The first row of MM gives angular momentum of whole system about heel
%   contact, with MM(1,:)*thetadotplus
%   The second row of MM gives angular momentum of trailing leg about hip
%   with MM(2,:)*thetadotplus

MM(1,1) = Il + Ml*R*(c1*(C - R) + R) + Mp*R*(c1*(L - R) + R) + ...
     Ml*R*(c2*(C - L) + c1*(L - R) + R) + Ml*(C - R)*(C - R + c1*R) + ...
      (-C + L)*Mp*(L - R + c1*R) + Mp*(C - R)*(L - R + c1*R) + ...
      (-C + L)*Ml*(c12*(C - L) + L - R + c1*R) + Ml*(C - R)*(c12*(C - L) + L - R + c1*R);
      
MM(1,2) = Il + (C - L)*Ml*(C - L + c12*(L - R) + c2*R);
      
MM(2,1) = c12*(C - L)*Ml*(L - R) + c2*(C - L)*Ml*R;

MM(2,2) = Il + Ml*((C - L)*(C - L));

unew = MM\amb;  % solve for thetadotplus with a linear system
% unew(1) is the leading leg,
% unew(2) is the trailing leg.

xnew = [xminus(2); xminus(1); unew(1); unew(2)];
% note legs are switched here

end % s2santhrowalk2

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

function [E, KE, PEN] = energyanthrowalk2(x, optparms);
% ENERGYANTHROWALK2  returns total energy of 2-segment, 2-D walker
% [E, KE, PEN] = energyanthrowalk2(x [,parms]) takes in the state vector and
% returns the total energy, kinetic energy, and potential
% energy of the walker for that state vector.
%
% If x is a 2D array with state vectors arranged in rows, then
% energy is computed for each row/state vector, and returned
% as a column of sequential energy values.

if nargin == 2 % parameters fed in as third argument
    parms = optparms;
end            % otherwise expect parms to be in scope

% Parameters: M L g gamma Mp Ml Ip Il C R
M = parms.M; L = parms.L; g = parms.g;
gamma = parms.gamma; Mp = parms.Mp; Ml = parms.Ml;
Ip = parms.Ip; Il = parms.Il; C = parms.C; R = parms.R;

if length(x) == 4  % Only 4 states
  q1 = x(1); q2 = x(2); u1 = x(3); u2 = x(4);
else               % Or an array of state vectors, each a row
  q1 = x(:,1); q2 = x(:,2); u1 = x(:,3); u2 = x(:,4);
end

c1 = cos(q1); s1 = sin(q1); c2 = cos(q2); s2 = sin(q2);
c12 = cos(q1-q2);
sg = sin(gamma);
cg = cos(gamma);
cgq1 = cos(gamma - q1); 

% The dot notation, e.g. ".*" allows element-by-element multiplication
% of arrays.

PEN = -(Ml*(-(cg*g*R) - g*q1*R*sg - g*(C - R)*cgq1)) - ...
   Mp*(-(cg*g*R) - g*q1*R*sg - g*(L - R)*cgq1) - ...
   Ml*(-(c2*cg*g*(C - L)) - cg*g*R - g*q1*R*sg - ...
      g*(C - L)*s2*sg - g*(L - R)*cgq1);   
KE = Il*(u1.*u1) + Ml*(2*c1.*((C - R)*R*(u1.*u1)) + (C - R)*(C - R)*(u1.*u1) + R*R*(u1.*u1))  + ...
     Mp*((-2*c1*R) .*u1 .*(-((-C + L)*u1) - (C - R)*u1) + R*R*(u1.*u1) + ...
        (-((-C + L)*u1) - (C - R)*u1).*(-((-C + L)*u1) - (C - R)*u1)) + Il*(u2.*u2) + ...
     Ml*((-2*c1*R).*u1.*(-((-C + L)*u1) - (C - R)*u1) + 2*c2.*((C - L)*R*u1.*u2) - ...
        2*(c12*(C - L)).*(-((-C + L)*u1) - (C - R)*u1).*u2 + R*R*(u1.*u1) + ...
        (-((-C + L)*u1) - (C - R)*u1).*(-((-C + L)*u1) - (C - R)*u1) + ...
        (C - L)*(C - L)*(u2.*u2));
        
 KE = KE * 0.5;
 E = KE + PEN;
 
end % energyanthrowalk2


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function zerror = fixedpointerrorsl(z, optparms)
% fixedpointerrorsl(z) returns zero when there is a fixed point at the
% desired step length. The input parameter is the difference between
% xnext and x0, that is, the difference in initial conditions
% for two successive steps.
% Finding the root of fixedpointpw is equivalent to finding a periodic gait.

if nargin == 2 % parameters fed in as 2nd argument
    parms = optparms;
end            % otherwise expect parms to be in scope

% Parameters: M L g gamma Mp Ml Ip Il C R
L = parms.L; R = parms.R; % only need these two

gamma = z(1); 
x0 = z(2:5);

parms.gamma = gamma;

xnext = onestepanthrowalk2(x0, parms);

steplength = 2*(L-R)*sin(xnext(1))+2*R*xnext(1);

zerror = [steplength - steplength0; xnext - x0 ]; 

end % fixedpointerrorsl

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function animateanthrowalk2(x,varargin)
% ANIMATEANTHROWALK2 for the 2-d passive walking simulation
%
% animateanthrowalk2(x [,parms]) with a list of state vectors (each in a row)
%   equally spaced in time, animates a single step. Parameters struct
%   parms must be in scope, or entered as optional argument.
%
% animateanthrowalk2(x, numsteps [,parms]) animates the single step given 
%   in x repetitively by a number of times specified by the scalar numsteps
%
% animateanthrowalk2(x, steplist [,parms]) animates a list of state vectors over 
%   multiple steps, where steplist is the list of the number of state
%   vectors (time steps) in each step.
%
% To have the program save each frame as an encapsulated Postscript file,
%   use animateanthrowalk2(x, numstepsORsteplist [,parms], 1)

% x should normally contain one full step of the states, [q1 q2 u1 u2] 
%   arranged in rows.  numsteps is the # of steps to walk (the states in x are
%   repeated automatically for numsteps > 1).  An alternative way to
%   call animatewalk is if x contains multiple steps, then numsteps
%   can be a vector containing the starting indices for each of the
%   multiple steps.

printflag = 0;  % default to not saving/printing frames
numsteps = 2;   % default to 2 steps animation

if nargin == 1 
    % expect parms to be in enclosing scope
elseif nargin == 2 % figure out if parms or numsteps
    if isstruct(varargin{1}) % parms struct
        parms = varargin{1};
    else
        numsteps = varargin{1};
        % parms should be scope
    end
elseif nargin == 3  % figure out if parms or printflag
    numsteps = varargin{1};
    if isstruct(varargin{2}) % parms struct
        parms = varargin{2};
    else
        printflag = varargin{2};
    end
elseif nargin == 4   % printflag is given
    numsteps = varargin{1};
    parms = varargin{2};
    printflag = varargin{3};
end

% Next need to figure out whether numsteps or steplist
xlen = length(x);

if length(numsteps) > 1 %  numsteps is a list 
  steplist = numsteps; % steplist is list of step time-lengths
  numsteps = length(steplist); % numsteps is just a count of steps
  endindex = cumsum(steplist); % within x, index the start and end
  startindex = [1 endindex+1]; % of each step
else % numsteps is just a scalar, so make our own steplist
  steplist = [xlen repmat(xlen-1, 1, numsteps-1)];
  endindex = repmat(xlen, numsteps, 1);
  startindex = repmat(1,numsteps,1); % extra frame
end
% Now numsteps contains the number of steps, steplist
% contains the number of frames in each step, and
% startindex and endindex contain indices for each step

% Parameters: M L g gamma Mp Ml Ip Il C R
M = parms.M; L = parms.L; g = parms.g;
gamma = parms.gamma; Mp = parms.Mp; Ml = parms.Ml;
Ip = parms.Ip; Il = parms.Il; C = parms.C; R = parms.R;

footn = 10; % arc foot will be drawn as this many line segments
alpha = 0.3; % with range of +/-alpha angle

pausetime = 0.5; % pause this much time between animation frames

debg = 0;  % set to 1 to display intermediate information


% Estimate range of walking
distance = 2*numsteps*R*x(1,1)+(numsteps+1)*((L-R)*abs(sin(x(1,1))-sin(x(1,2))));
xlimit = [-R distance+R]-(L-R)*abs(sin(x(1,1))-sin(x(1,2))); 
ylimit = [-0.05 1.35];

aang = pi/6; scale = 0.02; scale2 = 2; vx2 = 0.4; vy2 = 1.2;

% A foot

% foot starts at -sin(a),cos(a)
% and goes to sin(a),cos(a)
footang = linspace(-alpha*1.1, alpha*1.1, footn);
footxy = R*[sin(footang); -cos(footang)];

% Initialize
clf; 
q1 = x(1,1); q2 = x(1,2); u1 = x(1,3);
contactpoint = -q1*R;
Rot1 = [cos(q1) -sin(q1); sin(q1) cos(q1)];
Rot2 = [cos(q2) -sin(q2); sin(q2) cos(q2)];

footx1 = Rot1(1,:)*footxy + contactpoint; footy1 = Rot1(2,:)*footxy + R;
legsxy = [0  -sin(q1)  -sin(q1)+sin(q2);
  0   cos(q1)   cos(q1)-cos(q2)];
legsx = legsxy(1,:) + contactpoint + R*sin(q1);
legsy = legsxy(2,:) + R - R*cos(q1);       
footx2 = Rot2(1,:)*footxy + legsx(3) - R*sin(q2);
footy2 = Rot2(2,:)*footxy + legsy(3) + R*cos(q2);
pcm = legsxy(:,2) + [contactpoint+R*sin(q1);R-R*cos(q1)];
vcm = [-u1*(R + (L-R)*cos(q1)); -u1*(L-R)*sin(q1)];
velang = atan2(vcm(2),vcm(1));
velx = [0 vcm(1) vcm(1)-scale*cos(velang+aang) NaN vcm(1) vcm(1)-scale*cos(velang-aang)]+pcm(1);
vely = [0 vcm(2) vcm(2)-scale*sin(velang+aang) NaN vcm(2) vcm(2)-scale*sin(velang-aang)]+pcm(2);
velx2 = scale2*[0 vcm(1) vcm(1)-scale*cos(velang+aang) NaN vcm(1) vcm(1)-scale*cos(velang-aang)]+vx2;
vely2 = scale2*[0 vcm(2) vcm(2)-scale*sin(velang+aang) NaN vcm(2) vcm(2)-scale*sin(velang-aang)]+vy2;

set(gcf, 'color', [1 1 1]); set(gca,'DataAspectRatio',[1 1 1],'Visible','off','NextPlot','Add','XLim',xlimit,'YLim',ylimit);

hf1 = line(footx1,footy1,'LineWidth',3); 
hf2 = line(footx2,footy2,'LineWidth',3);
hlegs = line(legsx,legsy,'LineWidth',3);
hvel = line(velx,vely,'color','m','LineWidth',2);
hpelv = plot(legsx(2),legsy(2),'.','MarkerSize',30);
hgnd = line(xlimit,[0 0]-.01,'color',[0 0 0],'linewidth',2);
%hvel2 = line(velx2,vely2,'color','m','LineWidth',2);

th1old = q1; cntr = 1;
for j = 1:numsteps
  for i = startindex(j):endindex(j)
    q1 = x(i,1); q2 = x(i,2);
    contactpoint = contactpoint - (q1-th1old)*R; % roll forward a little
    th1old = q1;
    Rot1 = [cos(q1) -sin(q1); sin(q1) cos(q1)];
    Rot2 = [cos(q2) -sin(q2); sin(q2) cos(q2)];
    
    footx1 = Rot1(1,:)*footxy + contactpoint; footy1 = Rot1(2,:)*footxy + R;
    legsxy = [0  -sin(q1)  -sin(q1)+sin(q2);
              0   cos(q1)   cos(q1)-cos(q2)];
    legsx = legsxy(1,:) + contactpoint + R*sin(q1);
    legsy = legsxy(2,:) + R - R*cos(q1);       
    footx2 = Rot2(1,:)*footxy + legsx(3) - R*sin(q2);
    footy2 = Rot2(2,:)*footxy + legsy(3) + R*cos(q2);
    
    pcm = legsxy(:,2) + [contactpoint+R*sin(q1);R-R*cos(q1)];
    vcm = [-u1*(R + (L-R)*cos(q1)); -u1*(L-R)*sin(q1)];
    velang = atan2(vcm(2),vcm(1));
    velx = [0 vcm(1) vcm(1)-scale*cos(velang+aang) NaN vcm(1) vcm(1)-scale*cos(velang-aang)]+pcm(1);
    vely = [0 vcm(2) vcm(2)-scale*sin(velang+aang) NaN vcm(2) vcm(2)-scale*sin(velang-aang)]+pcm(2);
    velx2 = scale2*[0 vcm(1) vcm(1)-scale*cos(velang+aang) NaN vcm(1) vcm(1)-scale*cos(velang-aang)]+vx2;
    vely2 = scale2*[0 vcm(2) vcm(2)-scale*sin(velang+aang) NaN vcm(2) vcm(2)-scale*sin(velang-aang)]+vy2;
    
    set(hf1,'Xdata',footx1,'Ydata',footy1);
    set(hf2,'Xdata',footx2,'Ydata',footy2);
    set(hlegs,'Xdata',legsx,'Ydata',legsy);
    set(hvel,'Xdata',velx,'Ydata',vely);
    set(hpelv,'Xdata',legsx(2),'Ydata',legsy(2));
    if 0
    if i==1 & j > 1  % stick velocity arrow
      hveli=line(velx2,vely2,'color','m','LineWidth',2);
      oldx = get(hvelo,'xdata'); oldy = get(hvelo,'ydata');
      hsang = atan2(vely2(2)-oldy(2),velx2(2)-oldx(2));
      velxh = [oldx(2) velx2(2) velx2(2)-scale2*scale*cos(hsang+aang) NaN velx2(2) velx2(2)-scale2*scale*cos(hsang-aang)];
    	velyh = [oldy(2) vely2(2) vely2(2)-scale2*scale*sin(hsang+aang) NaN vely2(2) vely2(2)-scale2*scale*sin(hsang-aang)];  
  		hvelhs = line(velxh,velyh,'color','r','Linewidth',2);    
    end
    end
    drawnow; 
    if ~printflag
      pause(0.05)
    else
      print('-depsc2',sprintf('walk%02d',cntr));
    end
    if debg, pause, end;
    cntr = cntr + 1;
  end
  contactpoint = contactpoint - (L-R)*(sin(q1)-sin(q2)); th1old = q2;
end

end % animateanthrowalk2

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [xstar, cnvrg] = findroot(f, x0, frparms)
% FINDROOT  Finds the root of a vector function, with vector-valued x0.
%  
% xstar = findroot(f, x0) performs a Newton search and returns xstar,
%   the root of the function f, starting with initial guess x0. The
%   function will typically be expressed with a function handle, e.g. 
%   @f.
%
%   Optional input findroot(f, x0, frparms) includes parameters structure,
%   with fields dxtol (smallest allowable dx) and maxiter (max #
%   iterations).
%
%   Optional second output [xstar, cnvrg] = findroot... signals
%   successful convergence. It is set to 
%   false if maximum iterations is exceeded.

if nargin < 3
    frparms.dxtol = 1e-6;      % default tolerance for min change in x
    frparms.dftol = 1e-6;      % default tolerance for min change in f
    frparms.maxiter = 1000;    % allow max of 1000 iterations before quitting
    frparms.finitediffdx = []; % finite differencing step size
end

% We will take steps to improve on x, while monitoring
% the change dx, and stopping when it becomes small
dx = Inf;          % initial dx is large
iter = 0;          % count the iterations we go through
x = x0(:);         % start at this initial guess (treat x as a column vector)
fprevious = f(x0); % use to compare changes in function value
df = Inf;          % change in f is initialized as large

% Loop through the refinements, checking to make sure that x and f
% change by some minimal amount, and that we haven't exceeded
% the maximum number of iterations

while max(abs(dx)) >= frparms.dxtol & max(abs(df)) >= frparms.dftol & ...
        iter <= frparms.maxiter
  % Here is the main Newton step
  J = fjacobian(f, x, frparms.finitediffdx); % This is the slope
  dx = J\(0 - f(x));                 % and this is the correction to x
  x = x + dx;

  % Update information about changes
  iter = iter + 1;
  df = f(x) - fprevious;
  fprevious = f(x);
end

xstar = x; % use the latest, best guess

cnvrg = true;

if iter > frparms.maxiter % we probably didn't find a good solution
  warning('Maximum iterations exceeded in findroot');
  cnvrg = false;
end

if size(x0,2) > 1   % x0 was given to us as a row vector
    xstar = xstar'; % so return xstar in the same shape
end
    
end % findroot

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

function dfdx = fjacobian(f, x0, dx)
% FJACOBIAN   Computes Jacobian (partial derivative) of function
%
% dfdx = fjacobian(@f, x0 [, dx])
%
%   Uses finite differences to compute partial derivative of vector 
%   function f evaluated at vector x0. 
%   Optional argument dx specifies finite difference
%   step. Note that argument f should typically be entered as a
%   function handle, e.g. @f.
%
%   When dx is not given or empty, a default of 1e-6 is used.

if nargin < 3 || isempty(dx)
dx = 1e-6;
end

f0 = f(x0);
J = zeros(length(f0), length(x0));
for i = 1:length(x0)
xperturbed = x0;
xperturbed(i) = xperturbed(i) + dx;
df(:,i) = f(xperturbed) - f0;
end
dfdx = df / dx;

end % fjacobian function

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

end % outer function