Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_HHT/initialize_dynamic_data_6DOF.m
73754adfee53master

Last Change
Raw File

initialize_dynamic_data_6DOF.m
View Options

function [DYDATA] = initialize_dynamic_data_6DOF()
%{
Function for deriving the equations of motion of the joint model with 6DOF.
--------------------------------------------------------------------------
Syntax :
[DYDATA] = initialize_dynamic_data_6DOF()
--------------------------------------------------------------------------
Function Description :
--------------------------------------------------------------------------
%}
% define the generalized coordinates
syms q1 q2 q3 q4 q5 q6 real; %generalized coordinates
% psi theta phi x y z
syms dq1 dq2 dq3 dq4 dq5 dq6 real; %generalized velocities
syms ddq1 ddq2 ddq3 ddq4 ddq5 ddq6 real; %generalized accelerations
syms qg real; % generalized coordinate of the glenoid (scapula)
% and the generalized coordinates/velocities/accelerations vectors
q = [q1; q2; q3; q4; q5; q6];
dq = [dq1; dq2; dq3; dq4; dq5; dq6];
ddq = [ddq1; ddq2; ddq3; ddq4; ddq5; ddq6];
% define the constants
syms Ixx Iyy Izz M CG g real;
% inertia tensor around the center of mass
I = [Ixx 0 0; 0 Iyy 0; 0 0 Izz];
% define the rotation matrix for an Euler's sequence of YXY
R_psi = [cos(q1) 0 sin(q1);
0 1 0;
-sin(q1) 0 cos(q1)];
R_theta = [1 0 0;
0 cos(q2) -sin(q2);
0 sin(q2) cos(q2)];
R_phi = [cos(q3) 0 sin(q3);
0 1 0;
-sin(q3) 0 cos(q3)];
R_H2T = R_phi*R_theta*R_psi; % humerus to thorax (body-fixed to inertial)
R_G2T = [1 0 0;
0 cos(qg) -sin(qg);
0 sin(qg) cos(qg)]; % glenoid to thorax
% define the spatial configuration of the body (humerus) by the position
% of its center of mass in the inertial frame
r_HH = [q4 q5 q6]'; % position vector of the humeral head
% (tranlational vector) in thorax
OG = r_HH + R_H2T*[0 -CG 0]';
% angular velocity in the body-fixed frame:
Wrotation = dq3*R_psi'*R_theta'*[0 1 0]' + dq2*R_psi'*[1 0 0]' + dq1*[0 1 0]';
% angular velocity in the inertial frame:
Wrotation = simplify(expand(R_H2T*Wrotation));
% linear acceleration and velocity of the center of mass
V_CG = simplify(expand(jacobian(OG,q)*dq)); % linear velocity of the CG
A_CG = simplify(expand(jacobian(V_CG,q)*dq + jacobian(V_CG,dq)*ddq));
% Kinetic Energy
T = 0.5*M*[dq4 dq5 dq6]*[dq4 dq5 dq6]' + 0.5*Wrotation'*I*Wrotation;
% Potential energy
V = M*g*OG(2);
% Lagrangian
L = T - V;
% Derivations
MDYN = simplify(jacobian(jacobian(L,dq),dq));
RHS = simplify(-jacobian(jacobian(L,dq),q)*dq + jacobian(L,q)');
% save
[N1,N2]=size(MDYN);
N3=length(RHS);
fileID = fopen('EquationsofMotion_6DOF.m','w');
for i=1:N1
for j=1:N2
fprintf(fileID,'MDYN(%d,%d) = %s;\n', i, j, char(MDYN(i,j)));
end
end
for i=1:N3
fprintf(fileID,'RHS(%d,1) = %s;\n', i, char(RHS(i)));
end
fclose(fileID);
N4=length(A_CG);
fileID = fopen('LinearAccofCenterofMass_6DOF.m','w');
for i=1:N4
fprintf(fileID,'A_CG(%d,1) = %s;\n', i, char(A_CG(i)));
end
fclose(fileID);
%{
% define the generalized forces
Dir_inT = sym('Dir_inT',[3 6], 'real'); % muscle-force direction
Rho_inT = sym('Rho_inT',[3 6], 'real'); % muscle leverarm
F_final = sym('F_final',[1 6], 'real'); % muscle-force magnitude
Dir_contact = sym('Dir_contact',[3 20], 'real'); % contact-force direction
Rho_contact = sym('Rho_contact',[3 20], 'real'); % contact-force leverarm
F_contact = sym('F_contact',[1 20], 'real'); % contact-force magnitude
V_HH = [dq4 dq5 dq6]'; % the velocity of HH
F_resultant = zeros(3,1); % initialize the resultant of forces
M_resultant = zeros(3,1); % initialize the resultant of moments
for j = 1:size(F_final,2) + size(F_contact,2) % the number of forces applying on the humerus
if le(j,size(F_final,2))
F_resultant = F_final(j)*Dir_inT(:,j) + F_resultant;
M_resultant = cross(Rho_inT(:,j),F_final(j)*Dir_inT(:,j)) + M_resultant;
else
F_resultant = F_contact(j-size(F_final,2))*Dir_contact(:,j-size(F_final,2)) + F_resultant;
M_resultant = cross(Rho_contact(:,j-size(F_final,2)),F_contact(j-size(F_final,2))*Dir_contact(:,j-size(F_final,2))) + M_resultant;
end
end
for i = 1:6 % the number of generalized coordinates
Q(i,1) = F_resultant'*jacobian(V_HH,dq(i)) + M_resultant'*jacobian(Wrotation,dq(i));
end
% perform some substitutions
for i=1:6
str=char(Q(i));
for j = 1:9 % the largest one digit number
str = strrep(str, ['1' '_' num2str(j)], ['(' '1' ',' num2str(j) ')']);
str = strrep(str, ['2' '_' num2str(j)], ['(' '2' ',' num2str(j) ')']);
str = strrep(str, ['3' '_' num2str(j)], ['(' '3' ',' num2str(j) ')']);
str = strrep(str, ['l' num2str(j)], ['l' '(' num2str(j) ')']);
str = strrep(str, ['F_contact' num2str(j)], ['F_contact' '(' num2str(j) ')']);
end
for j = 0:9
str = strrep(str, [')' num2str(j)], [num2str(j) ')']);
end
Q(i)=cellstr(str);
end
fileID = fopen('GeneralizedForces_6DOF.m','w');
for i=1:6
fprintf(fileID,'Q(%d,1) = %s;\n',i,char(Q(i)));
end
fclose(fileID);
%}
% Defining the System Parameters
M = 3.82; %[Kg]
Ixx = 0.003+0.3914; %[Kgm2]
Iyy = 0.024; %[Kgm2]
Izz = 0.003+0.3914; %[Kgm2]
g = 9.82;
CG = 0.32; %[m]
% Rigid bodies properties
DYDATA.Parameters_6DOF = [M, g, CG;
Ixx, Iyy, Izz;
0, 0, 0];
return
c4science · Help