% climber.m
% Solution to problem 1 of Computation control 2 of 2017/02/13
% J.M.Soler, Feb.2017

% Set parameters
clear all
g = 9.81;         % gravitational acceleration (m/s^2)
m = 70;           % climber mass (kg)
d1 = 1.2;         % climber 'legs' length (from body center to feet), in m;
d2 = 1.0;         % climber 'arms' length (from body center to hands), in m;
d  = 2.0;         % distance from feet to hands (m)
phi = 120*pi/180; % wall inclination from horizontal (rad)
taumin = -100;    % min. body torque between arms and legs (N*m)
taumax = +500;    % max. body torque between arms and legs (N*m)
dtau = 10;        % torque increment for plot (N*m)

% Find body geometry
[r1,r2] = climberPosition( d1, d2, d, phi );

% Find climber forces, as a function of body torque
tau = (taumin:dtau:taumax);
ntau = numel(tau);
for itau = 1:ntau
  [f1,f2] = climberForce( m, d1, d2, d, phi, tau(itau) );
  f2norm(itau) = norm(f2);
end

% Select forces with no torque and with optimal torque
itau0 = find( abs(tau)==min(abs(tau)) );  % point at which tau=0
itau  = find( f2norm==min(f2norm) );      % point of min. hands force
[f10,f20] = climberForce( m, d1, d2, d, phi, tau(itau0) );
[f1,f2]   = climberForce( m, d1, d2, d, phi, tau(itau) );
fprintf(' Body torque = %4.1f kp*m => Hands force = %4.1f kp\n', ...
        tau(itau0)/g,norm(f20)/g)
fprintf(' Body torque = %4.1f kp*m => Hands force = %4.1f kp\n', ...
        tau(itau)/g,norm(f2)/g);

% Plot geometry and forces, with and without body torque
% Notice that this goes beyond what was requested in control
figure(1)
subplot(1,2,1)   % without body torque
plot([r1(1),0,r2(1)],[r1(2),0,r2(2)],[r1(1),r2(1)],[r1(2),r2(2)],'--')
hold on
quiver(0,0,0,-1,'r')
quiver(r1(1),r1(2),f10(1)/(m*g),f10(2)/(m*g),'r')
quiver(r2(1),r2(2),f20(1)/(m*g),f20(2)/(m*g),'r')
xlabel('x (m),   f_x/(m*g)')
ylabel('y (m),   f_y/(m*g)')
hold off
axis equal
axis([-0.5,1.2,-1,2])
title('No body torque')
subplot(1,2,2)  % with body torque
plot([r1(1),0,r2(1)],[r1(2),0,r2(2)],[r1(1),r2(1)],[r1(2),r2(2)],'--')
hold on
quiver(0,0,0,-1,'r')
quiver(r1(1),r1(2),f1(1)/(m*g),f1(2)/(m*g),'r')
quiver(r2(1),r2(2),f2(1)/(m*g),f2(2)/(m*g),'r')
xlabel('x (m),   f_x/(m*g)')
ylabel('y (m),   f_y/(m*g)')
hold off
axis equal
axis([-0.5,1.2,-1,2])
title('Optimal body torque')

% Plot hands forces as a function of body torque
figure(2)
plot(tau/g,f2norm/g)
xlabel('Body torque (kp*m)')
ylabel('Hands force (kp)')
grid on
