% Mechanism Kinematics
%  18.4.11 in text
% -Andy Ruina   May 5, 2021
%  See lecture notes for variable names
% 
clear all

% Need to "declare" all of the given variables and parameters
syms r d L  theta thetadot thetaddot real

 % right hand sides are all known or already calculated
 h   = r * sin(theta); 
 w   = r * cos(theta);
 H   = d- h;
 D   = sqrt(L^2 - H^2);

 xD   = D - w;  % position of mass
 
 % Used for chain rule (see lecture notes or video)
 dxDdth      = diff( xD    ,theta );  % Derivative of xD w.r.t. theta
                                 dxdth   = simplify(dxDdth);
 d2xDdth2    = diff( dxDdth,theta );    % Second derivative
                                 d2xdth2 = simplify(d2xDdth2);
 
 vD = dxdth * thetadot;  % not used 
 
 %Probably too complicated to do reliably by hand.
 aD  = d2xDdth2 * thetadot^2  +  dxDdth*thetaddot; % This is a mess

 
% Put in some numbers
 r= 1; d = 2; L = 3; theta= pi/2; thetadot = 4; thetaddot = 4;
 
 eval(aD)