%A MATLAB script to generate the equations of motion for RABBIT, a
%5-link, planar, kneed, biped walker during flight phase using the
%method of Lagrange.
%
%Copyright (c) 2002 by Eric Westervelt and Jessy Grizzle.  This
%code may be freely used for noncommercial ends. If use of this
%code in part or in whole results in publication, proper citation
%must be included in that publication.  This code comes with no
%guarantees or support.
%

% This is for RABBIT, robot with an upright torso, two legs and knees.
% The model is for five degrees of freedom of the robot, with the both
% leg ends not interacting with the ground.  The notation used for the
% equation of motions are as in Robot Dynamics and Control by Spong
% and Vidyasagar (1989), page 142, Eq. (6.3.12)
%
%    D(q)ddq + C(q,dq)*dq + G(q) = B*tau + E2*F_external
%
% Note the following convention
%
%  fem = femur, tib = tibia, 1 = stance leg, 2 = swing leg
%
% For simplicity, the model is derived using absolute coorinates
% measured in the trigonometric sense from the vertical.  The model is
% trasnformed in to relative coordates plus one absolute coordinates
% via a coordinate transformation.
%
% Eric Westervelt, Wed Sept 25  11:21:33 EST 2002

clear

% -----------------------------------------------------------------
%
%  Model variables
%
% -----------------------------------------------------------------

% absolute joint angles and velocities
syms q_torso dq_torso real
syms q_fem1 dq_fem1 real
syms q_fem2 dq_fem2 real
syms q_tib1 dq_tib1 real
syms q_tib2 dq_tib2 real

% gravity
syms g real

% link lengths
syms L_torso real
syms L_fem real
syms L_tib real

% link masses
syms M_torso real 
syms M_fem real 
syms M_tib real

% link inertias
syms MZ_torso real
syms MZ_fem real
syms MZ_tib real

% center of mass offsets
syms XX_torso real
syms XX_fem real
syms XX_tib real 

% position and velocity of the hips
syms xH yH dxH dyH

% relative joint angles
syms q31L q32L q41L q42L q1L real
q  = [q31L; q32L; q41L; q42L; q1L; xH; yH];

% relative joint velocities
syms dq31L dq32L dq41L dq42L dq1L real
dq = [dq31L; dq32L; dq41L; dq42L; dq1L; dxH; dyH];

% -----------------------------------------------------------------
%
%  Change coordinates to relative coordinates
%
% -----------------------------------------------------------------

T = [ 1   0   0   0   1  0  0;
      0   1   0   0   1  0  0;
      1   0   1   0   1  0  0;
      0   1   0   1   1  0  0;
      0   0   0   0   1  0  0];

% old absolute coordinates in terms of relative coords
q_new  = T*q;
dq_new = T*dq;

q_fem1  = q_new(1);
q_fem2  = q_new(2);
q_tib1  = q_new(3);
q_tib2  = q_new(4);
q_torso = q_new(5);
dq_fem1  = dq_new(1);
dq_fem2  = dq_new(2);
dq_tib1  = dq_new(3);
dq_tib2  = dq_new(4);
dq_torso = dq_new(5);

% -----------------------------------------------------------------
%
%  Calculate kinetic energy
%
% -----------------------------------------------------------------

% hip and knee positions
p_hip  = [xH; yH];
p_knee1 = p_hip + L_fem*[-sin(q_fem1); cos(q_fem1)];
p_knee2 = p_hip + L_fem*[-sin(q_fem2); cos(q_fem2)];

% hip and knee velocities
v_hip  = jacobian(p_hip,q)*dq;
v_knee1 = jacobian(p_knee1,q)*dq;
v_knee2 = jacobian(p_knee2,q)*dq;

% relative angular velocties -- needed since the centers of masses
% of the links are one collocated with the link reference frames
R_torso = [cos(q_torso) -sin(q_torso);
	   sin(q_torso) cos(q_torso)];
v_torso = R_torso.'*v_hip*dq_torso;

R_fem1 = [cos(q_fem1) -sin(q_fem1);
	  sin(q_fem1) cos(q_fem1)];
v_fem1 = R_fem1.'*v_hip*dq_fem1;

R_fem2 = [cos(q_fem2) -sin(q_fem2);
	  sin(q_fem2) cos(q_fem2)];
v_fem2 = R_fem2.'*v_hip*dq_fem2;

R_tib1 = [cos(q_tib1) -sin(q_tib1);
	  sin(q_tib1) cos(q_tib1)];
v_tib1 = R_tib1.'*v_knee1*dq_tib1;

R_tib2 = [cos(q_tib2) -sin(q_tib2);
	  sin(q_tib2) cos(q_tib2)];
v_tib2 = R_tib2.'*v_knee2*dq_tib2;

% kinetic energy of links
KE_torso = 1/2*M_torso*v_hip.'*v_hip ...
	  + v_torso.'*[-MZ_torso; 0]  ...
	  + 1/2*XX_torso*dq_torso^2;
KE_torso = simplify(KE_torso);
KE_fem1 = 1/2*M_fem*v_hip.'*v_hip ...
	 + v_fem1.'*[-MZ_fem; 0] ...
	 + 1/2*XX_fem*(dq_fem1)^2;
KE_fem1    = simplify(KE_fem1);
KE_fem2 = 1/2*M_fem*v_hip.'*v_hip ...
	 + v_fem2.'*[-MZ_fem; 0] ...
	 + 1/2*XX_fem*(dq_fem2)^2;
KE_fem2    = simplify(KE_fem2);
KE_tib1 = 1/2*M_tib*v_knee1.'*v_knee1 ...
	 + v_tib1.'*[-MZ_tib; 0] ...
	 + 1/2*XX_tib*(dq_tib1)^2;
KE_tib1 = simplify(KE_tib1);
KE_tib2 = 1/2*M_tib*v_knee2.'*v_knee2 ...
	 + v_tib2.'*[-MZ_tib;0] ...
	 + 1/2*XX_tib*(dq_tib2)^2;
KE_tib2 = simplify(KE_tib2);

% total kinetic energy
KE = KE_torso + KE_fem1 + KE_fem2 + KE_tib1 + KE_tib2;
KE = simple(KE);

% -----------------------------------------------------------------
%
%  Calculate potential energy
%
% -----------------------------------------------------------------

% positions of various members 
p_torso = p_hip + MZ_torso/M_torso*[-sin(q_torso); cos(q_torso)];
p_fem1  = p_hip + MZ_fem/M_fem*[-sin(q_fem1); cos(q_fem1)];
p_fem2  = p_hip + MZ_fem/M_fem*[-sin(q_fem2); cos(q_fem2)];
p_tib1  = p_knee1 + MZ_tib/M_tib*[-sin(q_tib1); cos(q_tib1)];
p_tib2  = p_knee2 + MZ_tib/M_tib*[-sin(q_tib2); cos(q_tib2)];
p_foot1 = p_knee1 + L_tib*[-sin(q_tib1); cos(q_tib1)];
p_foot2 = p_knee2 + L_tib*[-sin(q_tib2); cos(q_tib2)];

% total potential energy
PE = g*(M_torso*p_torso(2) + M_fem*p_fem1(2) + M_fem*p_fem2(2) + ...
	M_tib*p_tib1(2) + M_tib*p_tib2(2));
PE = simple(PE);

% -----------------------------------------------------------------
%
%  Calculate model matrices
%
% -----------------------------------------------------------------

% gravity vector
G = jacobian(PE,q).';
G = simple(G);

% mass-inertial matrix
D = simple(jacobian(KE,dq).');
D = simple(jacobian(D,dq));

% Coriolis and centrifugal matrix
syms C real
n=max(size(q));
for k=1:n
  for j=1:n
    C(k,j)=0*g;
    for i=1:n
      C(k,j)=C(k,j)+1/2*(diff(D(k,j),q(i)) + ...
			 diff(D(k,i),q(j)) - ...
			 diff(D(i,j),q(k)))*dq(i);
    end
  end
end
C=simple(C);

% input matrix
Phi_0 = [q_fem1-q_torso;
	 q_fem2-q_torso;
	 q_tib1-q_fem1;
	 q_tib2-q_fem2;
	 q_torso];
B = jacobian(Phi_0,q);
B = B.'*[eye(4,4);zeros(1,4)];

% swing foot force input matrix (F_ext = [F_T;F_N])
Phi_1 = [p_foot2];
E = jacobian(Phi_1,q).';

% -----------------------------------------------------------------
%
%  Print the model to an m-file to permit numerical evaluation
%
% -----------------------------------------------------------------

N = length(q);
fcn_name = 'RABBIT_flight_dynamics';
disp(['[creating ',fcn_name,'.m]']);
fid = fopen([fcn_name,'.m'],'w');
fprintf(fid,'function [D,C,G,B,E] = %s(x)\n',fcn_name);
fprintf(fid,'%% %s    The flight dynamics of RABBIT.\n', ...
	upper(fcn_name));
fprintf(fid,'%%    [D,C,G,B,E] = %s',upper(fcn_name));
fprintf(fid,'(X) is the dynamic model of\n');
fprintf(fid,'%%    RABBIT. (x is of dimension %s)\n', ...
	num2str(2*N));
fprintf(fid,'%%Eric Westervelt\n');
fprintf(fid,'%%%s\n\n',datestr(now));

% The model constants
fprintf(fid,'g        = 9.81;\n');
fprintf(fid,'L_torso  = 0.625;\n');
fprintf(fid,'L_fem    = 0.4;\n');
fprintf(fid,'L_tib    = 0.4;\n');
fprintf(fid,'M_torso  = 20;\n');
fprintf(fid,'M_fem    = 6.8;\n');
fprintf(fid,'M_tib    = 3.2;\n');
fprintf(fid,'MZ_torso = 4.0;\n');
fprintf(fid,'MZ_fem   = 1.11;\n');
fprintf(fid,'MZ_tib   = 0.41;\n');
fprintf(fid,'XX_torso = 2.22;\n');
fprintf(fid,'XX_fem   = 1.08;\n');
fprintf(fid,'XX_tib   = 0.93;\n\n');

% Reassign configuration parameters
fprintf(fid,['q31L=x(1); q32L=x(2); q41L=x(3); q42L=x(4); q1L=x(5);' ...
	     ' xH=x(6); yH=x(7);\n']);
fprintf(fid,['dq31L=x(8); dq32L=x(9); dq41L=x(10); dq42L=x(11); dq1L=' ...
	     'x(12); dxH=x(13); dyH=x(14);\n\n']);

% Model output
fprintf(fid,'%% D matrix\n');
fprintf(fid,'D = zeros(%s);\n',num2str(N));
for k = 1:N,
  for j = 1:N,
    if D(k,j) ~= 0
      ttt = char(D(k,j));
      fprintf(fid,'D(%s,%s) = %s;\n',num2str(k),num2str(j),ttt);
    end
  end
end

fprintf(fid,'\n%% C matrix\n');
fprintf(fid,'C = zeros(%s);\n',num2str(N));
for k = 1:N,
  for j = 1:N,
    if C(k,j) ~= 0
      ttt = char(C(k,j));
      fprintf(fid,'C(%s,%s) = %s;\n',num2str(k),num2str(j),ttt);
    end
  end
end

fprintf(fid,'\n%% G matrix\n');
fprintf(fid,'G = zeros(%s,1);\n',num2str(N));
for k = 1:N,
  if G(k) ~= 0
    ttt = char(G(k));
    fprintf(fid,'G(%s) = %s;\n',num2str(k),ttt);
  end
end

fprintf(fid,'\n%% B matrix\n');
[N,M] = size(B);
fprintf(fid,'B = zeros(%s,%s);\n',num2str(N),num2str(M));
for k = 1:N,
  for j = 1:M,
    if B(k,j) ~= 0
      ttt = char(B(k,j));
      fprintf(fid,'B(%s,%s) = %s;\n',num2str(k),num2str(j),ttt);
    end
  end
end

fprintf(fid,'\n%% E matrix\n');
[N,M] = size(E);
fprintf(fid,'E = zeros(%s,%s);\n',num2str(N),num2str(M));
for k = 1:N,
  for j = 1:M,
    if E(k,j) ~= 0
      ttt = char(E(k,j));
      fprintf(fid,'E(%s,%s) = %s;\n',num2str(k),num2str(j),ttt);
    end
  end
end
fclose(fid);