%A MATLAB script to generate the equations of motion for a 5-link,
%planar, kneed, biped walker using the method of Lagrange.
%
%Copyright (c) 2001 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 a robot with an upright torso, two legs and
% knees. The model is for five degrees of freedom of the robot,
% with the stance foot (Foot1) pinned to 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, Mon Aug 6 17:08:49 EST 2001
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 MY_torso real
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
% relative joint angles
syms q31L q32L q41L q42L q1L real
q = [q31L; q32L; q41L; q42L; q1L];
% relative joint velocities
syms dq31L dq32L dq41L dq42L dq1L real
dq = [dq31L; dq32L; dq41L; dq42L; dq1L];
% -----------------------------------------------------------------
%
% Change coordinates to relative coordinates
%
% -----------------------------------------------------------------
T = [ 1 0 0 0 1;
0 1 0 0 1;
1 0 1 0 1;
0 1 0 1 1;
0 0 0 0 1];
% old absolute coordinates in terms of relative coords
q_new = inv(T)*q;
dq_new = inv(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 = L_fem*[sin(q_fem1); -cos(q_fem1)] ...
+ L_tib*[sin(q_tib1); -cos(q_tib1)];
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; MY_torso] ...
+ 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 ...
+ 1/M_torso*[-sin(q_torso)*MZ_torso - cos(q_torso)*MY_torso;
cos(q_torso)*MZ_torso + sin(q_torso)*MY_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).';