%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 3 links balancing on a foot.
% goal is to put the "head" or tip at an x y location
% zero position has each link straight up.

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

syms aa ak ah real     % angles: ankle, knee, and hip
syms lshank lthigh ltorso real     % lengths
syms Ra Rk Rh real     % rotation matrices corresponding to ankle, knee, hip
syms lshankcm lthighcm ltorsocm real  % location of center of mass
      % from bottom of link
syms mshank mthigh mtorso real   % masses

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

% Ra (ankle) rotates vectors in the shank frame into the world frame
Ra = [ cos( aa ) -sin( aa )
       sin( aa ) cos( aa ) ]

% Rk (knee) rotates vectors in the thigh frame into the shank frame
Rk = [ cos( ak ) -sin( ak )
       sin( ak ) cos( ak ) ]

% Rh (hip) rotates vectors in the torso frame into the thigh frame
Rh = [ cos( ah ) -sin( ah )
       sin( ah ) cos( ah ) ]

link1 = [ 0
          lshank ]

link2 = [ 0
          lthigh ]

link3 = [ 0
          ltorso ]

link1cm = [ 0
            lshankcm ]

link2cm = [ 0
            lthighcm ]

link3cm = [ 0
            ltorsocm ]

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

tip = Ra * ( link1 + Rk * ( link2 + Rh * link3 ) )

tip = simplify(tip)
%   - lthigh*sin(aa + ak) - lshank*sin(aa) - ltorso*sin(aa + ah + ak)
%    lthigh*cos(aa + ak) + lshank*cos(aa) + ltorso*cos(aa + ah + ak)

%try
%   simple(tip)

% compute Jacobian
jca = diff( tip, aa )
jck = diff( tip, ak )
jch = diff( tip, ah )

J = [ jca jck jch ]
%   [ -lthigh*cos(aa + ak) - lshank*cos(aa) - ltorso*cos(aa + ah + ak),
%      -lthigh*cos(aa + ak) - ltorso*cos(aa + ah + ak),
%       -ltorso*cos(aa + ah + ak)]
%   [ -lthigh*sin(aa + ak) - lshank*sin(aa) - ltorso*sin(aa + ah + ak),
%      -lthigh*sin(aa + ak) - ltorso*sin(aa + ah + ak),
%       -ltorso*sin(aa + ah + ak)]

% angle_update = -small_number*J'*tip_error

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% COM (center of mass)
% we want to move tip to target while keeping COM above foot

com1 = simplify( Ra * link1cm )
com2 = simplify( Ra * ( link1 + Rk * link2cm ) )
com3 = simplify( Ra * ( link1 + Rk * ( link2 + Rh * link3cm ) ) )

total_mass = mshank + mthigh + mtorso
com = (mshank*com1 + mthigh*com2 + mtorso*com3)/total_mass

com(1) % X coordinate of center of mass
% -(mthigh*(lthighcm*sin(aa + ak) + lshank*sin(aa))
%  + mtorso*(lthigh*sin(aa + ak) + lshank*sin(aa)
%   + ltorsocm*sin(aa + ah + ak)) + lshankcm*mshank*sin(aa))
%    /(mshank + mthigh + mtorso)

score = com(1)*com(1) % penalize deviations of COMx from 0

% gradient of score
grad = [ diff( score, aa ) diff( score, ak ) diff( score, ah ) ]

grad = simplify( grad )
% [ (2*(mthigh*(lthighcm*cos(aa + ak) + lshank*cos(aa))
%    + mtorso*(lthigh*cos(aa + ak) + lshank*cos(aa)
%     + ltorsocm*cos(aa + ah + ak))
%      + lshankcm*mshank*cos(aa))*(mthigh*(lthighcm*sin(aa + ak)
%       + lshank*sin(aa)) + mtorso*(lthigh*sin(aa + ak) + lshank*sin(aa)
%        + ltorsocm*sin(aa + ah + ak))
%         + lshankcm*mshank*sin(aa)))/(mshank + mthigh + mtorso)^2,
%   (2*(mtorso*(lthigh*cos(aa + ak) + ltorsocm*cos(aa + ah + ak))
%    + lthighcm*mthigh*cos(aa + ak))*(mthigh*(lthighcm*sin(aa + ak)
%     + lshank*sin(aa)) + mtorso*(lthigh*sin(aa + ak) + lshank*sin(aa)
%      + ltorsocm*sin(aa + ah + ak)) + lshankcm*mshank*sin(aa)))
%       /(mshank + mthigh + mtorso)^2,
%   (2*ltorsocm*mtorso*cos(aa + ah + ak)*(mthigh*(lthighcm*sin(aa + ak)
%    + lshank*sin(aa)) + mtorso*(lthigh*sin(aa + ak) + lshank*sin(aa)
%     + ltorsocm*sin(aa + ah + ak)) + lshankcm*mshank*sin(aa)))
%      /(mshank + mthigh + mtorso)^2]

% angle_update = -small_number*J'*tip_error - another_small_number*grad'

% increase another_small_number relative to small_number to move COMx
% closer to zero.

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