% tutorial3.m
%
% Stiffness analysis
% Created 06/2010 by Lael Odhner <lael.odhner@yale.edu>

% start by running the script from the previous example to create a robot
% object.
tutorial2;

% Zero the force from the previous example.
r.ES_Mode('tip').SetForce([0; 0; 0]);

% Let's assume that the arm is straight out
q2 = zeros(2,1);

% We'll define some servo stiffness for the arm, 100 Nm/rad at each joint
K = 100*eye(2);
r.Add_ES_Mode('servo_stiffness', 'symbolic', 0.5*q.'*K*q);

% compute the compliance
C = r.Compliance(q2);

% We can calculate the compliance of the endpoint for this servo stiffness
C_tip = r.ComplianceAtPoint(q2, Link2_Dist, 1:2);

% Now let's look for buckling modes. 
N = 100;
F = linspace(0, 100, N);
C = zeros(1, N);

for k=1:N
    r.ES_Mode('tip').SetForce([-F(k); 0; 0]);
    C(k) = r.ComplianceAtPoint(zeros(2,1), Link2_Dist, 2);
end

figure(2);
clf;
plot(F, 1./C);
xlabel('Compressive force (N)');
ylabel('Stiffness (N/m)');