Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_3DOF/initialize_dynamic_data.m
ece361673202master

Last Change
Raw File

initialize_dynamic_data.m
View Options

function [DYDATA] = initialize_dynamic_data()
%{
Function for deriving the equations of motion.
--------------------------------------------------------------------------
Syntax :
[DYDATA] = initialize_dynamic_data()
--------------------------------------------------------------------------
Function Description :
--------------------------------------------------------------------------
%}
% Let's define the generalized coordinates
syms q1 q2 q3 real; %generalized coordinates
syms dq1 dq2 dq3 real; %generalized velocities
syms ddq1 ddq2 ddq3 real; %generalized accelerations
% and the generalized coordinates/velocities/accelerations vectors
q = [q1; q2; q3];
dq = [dq1; dq2; dq3];
ddq = [ddq1; ddq2; ddq3];
% Define the constants
syms Ixx Iyy Izz M CG g real;
% Inertia tensor
I = [Ixx 0 0; 0 Iyy 0; 0 0 Izz];
% Defining the rotation matrix for a typical 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)];
Ryxy = R_phi*R_theta*R_psi; % local to global
% we define the spatial configuration of the body by the position of its
% center of mass in the inertial frame
OG = Ryxy*[0; -CG; 0];
% here is the angular velocity of the body in the body fixed (humerus) frame:
Wrotation = dq3*R_psi'*R_theta'*[0 1 0]' + dq2*R_psi'*[1 0 0]' + dq1*[0 1 0]';
% angular velocity of the body in the inertial frame (thorax):
Wrotation = simplify(expand(Ryxy*Wrotation));
% linear acceleration 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*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.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.m','w');
for i=1:N4
fprintf(fileID,'A_CG(%d,1) = %s;\n', i, char(A_CG(i)));
end
fclose(fileID);
% Generalized forces
Dir_inT = sym('Dir_inT',[3 6], 'real'); % direction
Rho_inT = sym('Rho_inT',[3 6], 'real'); % OP
F_final = sym('F_final',[1 6], 'real'); % magnitude
% partial angular velocities in the Thorax frame (dWrotation/ddq)
e = [sin(q2)*sin(q3) cos(q2) cos(q3)*sin(q2);
cos(q3) 0 -sin(q3);
0 1 0];
% genealized forces
Q = e*[cross(Rho_inT(:,1),Dir_inT(:,1)), cross(Rho_inT(:,2),Dir_inT(:,2)),...
cross(Rho_inT(:,3),Dir_inT(:,3)), cross(Rho_inT(:,4),Dir_inT(:,4)),...
cross(Rho_inT(:,5),Dir_inT(:,5)), cross(Rho_inT(:,6),Dir_inT(:,6))]*...
F_final';
for i=1:3
str=char(Q(i));
str = strrep(str, 'al1', 'al(1)');
str = strrep(str, 'al2', 'al(2)');
str = strrep(str, 'al3', 'al(3)');
str = strrep(str, 'al4', 'al(4)');
str = strrep(str, 'al5', 'al(5)');
str = strrep(str, 'al6', 'al(6)');
str = strrep(str, '1_1', '(1,1)');
str = strrep(str, '1_2', '(1,2)');
str = strrep(str, '1_3', '(1,3)');
str = strrep(str, '1_4', '(1,4)');
str = strrep(str, '1_5', '(1,5)');
str = strrep(str, '1_6', '(1,6)');
str = strrep(str, '2_1', '(2,1)');
str = strrep(str, '2_2', '(2,2)');
str = strrep(str, '2_3', '(2,3)');
str = strrep(str, '2_4', '(2,4)');
str = strrep(str, '2_5', '(2,5)');
str = strrep(str, '2_6', '(2,6)');
str = strrep(str, '3_1', '(3,1)');
str = strrep(str, '3_2', '(3,2)');
str = strrep(str, '3_3', '(3,3)');
str = strrep(str, '3_4', '(3,4)');
str = strrep(str, '3_5', '(3,5)');
str = strrep(str, '3_6', '(3,6)');
Q(i)=cellstr(str);
end
fileID = fopen('GeneralizedForces.m','w');
for i=1:3
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.81;
CG = 0.32; %[m]
% Rigid bodies properties
DYDATA.Parameters = [M, g, CG;
Ixx, Iyy, Izz;
0, 0, 0];
return
c4science · Help