function lcp_homework_soln
% Solution based on finger motion control.  That is, solve the problem by
% thinking in terms of moving the lower left finger in it's normal
% direction.
%
% Here are the basic equations
% needed.  Recall h = m = J = 1, so the equations are simpler:
% nu_lp1 = Gn*pn + pext  (assume nu_l = 0)                       (1)
% rho_n_lp1 = Gn'*nu_lp1 + psi_n_l + part_psi_wrt_t              (2)
% 0 \le pn_lp1  \perp  rho_n_lp1 \ge 0                           (3)

% Set up the given quantities.    
%a = 0.1;  % value for part C
a = -0.1; % value for part D
Gn = [-0.707  0  1  1  0  0;
      0.707  -1  0  0  1  1;
         0    0 -a  1 -1  0];
pext = [0; -1; 0];
psi_n_l = [0.1*ones(4,1); 0 ; 0];
 
% Solve for nu_lp1 to assemble the parts.  Note that the equations for
% any three contacts are linearly independent, since no three contact
% normals intersect at a point.  Choose the equations for the immobile
% contacts. 
A3 = Gn(:,2:4)
nu_lp1 = -inv(A3')*psi_n_l(2:4);

% Now solve for the motion of the movable finger.  This solution for vn
% gives the motions of all the contacts in the normal directions to
% maintain contact.  This is why the 5th and 6th elements of vn are -0.1.
% It means that to maintain contact with the part, they would have to move
% upward.
vn = -Gn'*nu_lp1 - psi_n_l;

% The solution we want is the first element.
part_psi_wrt_t = zeros(6,1);
part_psi_wrt_t(1) = vn(1);
% Check to see that we have not chosen nu_lp1 or a finger motion that
%causes penetration.  rho_lp1 should equal zero at the first four contacts
%and be greater than zero for the other two.
rho_n_lp1 = Gn'*nu_lp1 + psi_n_l + part_psi_wrt_t;

% Substituting first eq(1) into eq(2) gives:
% rho_n_lp1 = Gn'*(Gn*pn + pext) + psi_n_l + part_psi_wrt_t
% Gn'*Gn is singular.  It has rank = 3, so there are three linearly
% independent equations. 
%
% Disregard the contacts we want to lose, so include only the first four
% columns of Gn.  Rank is still 3, so nullspace(Gn) is of dimension 1.
% Write the general solution for pn_lp1, including the null space.
A = Gn'*Gn;
A4 = A(1:4,1:4);
B4 = Gn(:,1:4);
% The null space of A4 has all positive elements, indicating that all
% contact forces rise as finger 1 squeezes.  Simply multiply be a large
% enough number to push all elements of pn_lp1 into positive values.
squeeze = 1.7;
pn4 = -pinv(A4)*(B4'*pext + psi_n_l(1:4) + part_psi_wrt_t(1:4)) + null(A4)*squeeze;
pn = [pn4; 0; 0]
% This is a check on my equations.
nu_lp1_final = Gn*pn + pext;
error = nu_lp1_final - nu_lp1

% For the second part of this problem, when contact 3 is moved downward,
% a solution with pn_lp1 \ge 0 is impossible.  This corresponds a grasp
% without form closure.  Gravity causes the object to break one of the four
% intended contacts even if you put the object and fingers in the right
% places to begin.  This is seen by noting that null(A4) has a negative
% element in a bad place.


