Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_HHT/state_space_generator_6DOF.m
73754adfee53master

Last Change
Raw File

state_space_generator_6DOF.m
View Options

function state_space_generator_6DOF(DYDATA)
%{
Function for constructing the state-space form.
--------------------------------------------------------------------------
Syntax :
[du] = state_space_generator(DYDATA)
--------------------------------------------------------------------------
Function Description :
This function build on the equations of motion derived already will
generate the state-space form of the equations of motion.
--------------------------------------------------------------------------
%}
% Define the states and the generalized forces
syms q1 q2 q3 q4 q5 q6 real; % generalized coordinates
syms dq1 dq2 dq3 dq4 dq5 dq6 real; % generalized velocities
syms Q1 Q2 Q3 Q4 Q5 Q6 real; % generalized forces
Tau = [Q1 Q2 Q3 Q4 Q5 Q6]'; % generalized force vector
% Quantify the constants
M = DYDATA.Parameters_6DOF(1,1); Ixx = DYDATA.Parameters_6DOF(2,1);
g = DYDATA.Parameters_6DOF(1,2); Iyy = DYDATA.Parameters_6DOF(2,2);
CG = DYDATA.Parameters_6DOF(1,3); Izz = DYDATA.Parameters_6DOF(2,3);
% Re-write the equations of motion introducing the values above
EquationsofMotion_6DOF
% Define the generalized accelerations
DDq = inv(MDYN)*(RHS + Tau);
% Saving algorithm
List_of_variables = {'DDq'};
CurrentPath = pwd;
File_name = [CurrentPath, '/GeneralizedAccelerations_6DOF.m'];
fid = fopen(File_name,'wt');
index_equ = 1;
for k = 1:length(List_of_variables)
Var_name = List_of_variables{k};
size_var = size(eval(Var_name));
for i = 1:size_var(1)
Var_name_i = strcat(Var_name,'(',num2str(i),')');
Equation = strcat(Var_name_i,' = ',char(eval(Var_name_i)),';');
fprintf(fid,'%s \n',Equation);
end
end
fclose(fid);
return;
c4science · Help