% This script runs for an UNSTABLE set of walker parameters.
% (KAB, 1/10/2010)

% First, set parameters for the walker:
par.L = .132; % [m], leg length
par.R = .06;  % [m], foot radius
par.m = .07;  % [kg], leg mass
par.I = 1.8e-4;  % [ kgm^2], leg inertia
par.B = 0;    % [m], CoM B (horizontal position on leg)
par.C = .061; % [m], CoM C (vertical position on leg)
par.gamma = .005; % [radians], ground slope
par.g = 9.81; % [m/s^2], gravity
%par   % display all parameter values...

if 0  % if you have the optimal control toolbox, you can set to "1"
   
        options=optimset('TolFun',1e-10,'TolX',1e-10,'LargeScale','off');
        s_star = lsqnonlin(@(s) Step(s,0,par)-s,s0,[],[],options);
    else
        s_star=[  0.1989;   -0.1989;   -2.4473;   -2.2811];  % unstable fixed pt...
end

% ============= HERE IS THE LOOP TO CREATE THE JACOBIAN! ============
delta = 10e-4;
for i=1:4
    s0 = s_star;
    s0(i) = s0(i) + delta;
    J(:,i) = (Step(s0, 0, par)-s_star)'/delta;
end
% ===================================================================

% It is STABLE if ALL eigenvalues have magnitude less than one:
[eig_vectors, eig_values] = eig(J)

s = s_star; % now, begin at this (approximate) fixed point
figure(3); clf
alpha = s(1)-s(2); % the "interleg" angle = theta_1 - theta_2
% plot the fixed point:
plot3(s(3),s(4),alpha,'ko','MarkerFaceColor','k'); % The initial condition...
hold on; grid on; axis vis3d
xlabel('$\dot{\theta_1}$','Interpreter','latex');
ylabel('$\dot{\theta_2}$','Interpreter','latex');
zlabel('$\alpha = \theta_1 - \theta_2$','Interpreter','latex');
for n=1:100  % Now, take 100 steps, and "see" if it is stable
    [s_end, t_end, data] = Step(s(:,n),0,par);
    s(:,n+1) = s_end;
    alpha(:,n+1) = s(1,n+1)-s(2,n+1);
    if n>1
        set(plast,'Color','b')
    end
    plast=plot3(s(3,[n n+1]),s(4,[n n+1]),alpha([n n+1]),'r.-');
    fprintf('Done with step %d.  Deltas from last step were:\n',n)
    fprintf('     %.2e %.2e %.2e %.2e\n',...
        s(3,n+1)-s(3,n),s(4,n+1)-s(4,n),alpha(n+1)-alpha(n));
    input('Hit ENTER to continue...')
end
