%A MATLAB script to generate the equations of motion for RABBIT, a %5-link, planar, kneed, biped walker during stance 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 % 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, Wed Sept 25 11:19:49 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 % 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 = 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 = 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; 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_stance_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 stance 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);\n']); fprintf(fid,['dq31L=x(6); dq32L=x(7); dq41L=x(8); dq42L=x(9); dq1L=' ... 'x(10);\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);