% DeriveEOM  derives symbolically the equations motion using the TMT-method.
%
%   See also: STEP, PRINTMATRIX, WALK, ANIMATION, ENERGYPLOT

% Daniël Karssen, Martijn Wisse and Arend Schwab
% Delft Biorobotics Lab, TUDelft, March 2008
% created for tutorial of Dynamicwalking 2008

% clear memory 
clear all
clc

% creation of symbolic variables
% leg parameters
syms  L     % leg length
syms  R     % foot radius
syms  B     % horizontal position of the CoM with resoect to the hip
syms  C     % vertical position of the CoM with respect to the hip

% generalized coordinates and their derivatives
syms  phi_st  phi_st_d  % counter-clockwise rotation of the stance leg
syms  phi_sw  phi_sw_d  % counter-clockwise rotation of the swing leg 

% creation of vector of generalized coordinates
q  = [phi_st;  phi_sw];     % generalized coordinates
qd = [phi_st_d; phi_sw_d];  % velocities of generalized coordinates

% auxiliary relationship: position of hip joint
pos_h = [-R*phi_st - (L-R)*sin(phi_st)
         R         + (L-R)*cos(phi_st)];

% expression of all model coordinates in terms of generalized coordinates
x = [ pos_h + rotationmatrix(phi_st)*[B; -C]    % x- and y position of CoM of the stance leg 
      phi_st                                    % angle of CoM of the stance leg 
      pos_h + rotationmatrix(phi_sw)*[B; -C]    % x- and y position of CoM of the swing leg 
      phi_sw                                ];  % angle of CoM of the swing leg    

% derivation of partial derivatives of x to q
T = simple(jacobian(x,q));
     
% derivation of corriolis terms (second derivative of x)
D = simple(jacobian(T*qd,q)*qd);

% print matrices so that the user can copy-paste them to Step.m
PrintMatrix(T,'T')
PrintMatrix(D,'D')


% for the impact equation we need also to express the model coordinates in
% terms of generalized coordinates with the assumption that the swing leg is
% in contact with the ground

% position of hip joint with repect to the swing leg
pos_h_sw = [-R*phi_sw - (L-R)*sin(phi_sw)
            R         + (L-R)*cos(phi_sw)];

% expression of all model coordinates in terms of generalized coordinates
x_sw = [ pos_h_sw + rotationmatrix(phi_st)*[B; -C]
         phi_st
         pos_h_sw + rotationmatrix(phi_sw)*[B; -C]
         phi_sw                                   ];

% derivation of partial derivatives of x to q
T_sw = simple(jacobian(x_sw,q));

% print matrix so that the user can copy-paste it to Step.m
PrintMatrix(T_sw,'T_sw')