Homec4science
Diffusion MSM (command_line) Browse

msm/ShoulderModel_HHT/StateSpace.m
73754adfee53command_line

Last Change
Raw File

StateSpace.m
View Options

function du = StateSpace(t, u, TimeID, IDDATA, DYDATA, CMDATA)
%{
Function for generating the state space.
--------------------------------------------------------------------------
Syntax :
[du] = StateSpace(t, u, TimeID, IDDATA)
--------------------------------------------------------------------------
Function Description :
This function consturcts the state space form of the equations of motion in
order to let the numerical solver of matlab handle the problem.
--------------------------------------------------------------------------
%}
q1 = u(1); q2 = u(2); q3 = u(3); q4 = u(4); q5 = u(5); q6 = u(6);
dq1 = u(7); dq2 = u(8); dq3 = u(9); dq4 = u(10); dq5 = u(11); dq6 = u(12);
% define the muscle forces and thier leverarms at time t of the integration
% TimeID = find(abs(IDDATA.ImposedMotion(4,:) - u(2)) < 1e-3); % if we want to have these value as a function of q
% for muscleID = 1:6
% Dir_inT(:, muscleID) = IDDATA.ForceDirection{TimeID(1)}(:,muscleID);
% Rho_inT(:, muscleID) = IDDATA.LeverArm{TimeID(1)}(:,muscleID);
% F_final(muscleID) = IDDATA.MuscleForces(muscleID,TimeID(1));
% end
%
for muscleID = 1:6
Dir_inT(:, muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),[IDDATA.ForceDirection{TimeID}(:,muscleID)';IDDATA.ForceDirection{TimeID+1}(:,muscleID)'],t)';
Rho_inT(:, muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),[IDDATA.LeverArm{TimeID}(:,muscleID)';IDDATA.LeverArm{TimeID+1}(:,muscleID)'],t)';
F_final(muscleID) = interp1(IDDATA.t(TimeID:TimeID+1),IDDATA.MuscleForces(muscleID,TimeID:TimeID+1),t);
end
% define the contact forces and their leverarms at time t of the
% integration. The difference comparing to the muscle forces is that these
% quantities are not explicit functions of time. However, they explicitly
% depend on the generalized coordinates {q}.
% Rot_H2T = versatile_function('define Rot_H2T',u); % define the rotation matrix
r_HH = [u(4) u(5) u(6)]'; % HH tranlational vector
% DRot_H2T = versatile_function('define DRot_H2T',u); % define the rate of change of the rotation matrix
Dr_HH = [u(10) u(11) u(12)]';
% chose the contact model
% [Dir_contact,Rho_contact,F_contact]= contact_model(Rot_H2T,r_HH,CMDATA);
% [Dir_contact,Rho_contact,F_contact]= contact_model_damped(Rot_H2T,DRot_H2T,r_HH,Dr_HH,CMDATA);
% [Dir_contact,Rho_contact,F_contact]= contact_model_dominique(Rot_H2T,DRot_H2T,r_HH,Dr_HH,CMDATA);
% [Dir_contact,Rho_contact,F_contact]= contact_model_damped_faster_try1(Rot_H2T,DRot_H2T,r_HH,Dr_HH,CMDATA);
[Dir_contact,Rho_contact,F_contact]= contact_model_portugease_1(r_HH, Dr_HH, CMDATA);
% calculate the generalized forces at time t of the integration
% Q = generalized_force(Dir_inT,Rho_inT,F_final,Dir_contact,Rho_contact,F_contact,u);
Q = generalized_force_4_portugease(Dir_inT,Rho_inT,F_final,Dir_contact,Rho_contact,F_contact,u);
Q1 = Q(1); Q2 = Q(2); Q3 = Q(3); Q4 = Q(4); Q5 = Q(5); Q6 = Q(6);
GeneralizedAccelerations_6DOF;
du(1) = u(7); du(4) = u(10);
du(7) = DDq(1); du(10) = DDq(4);
du(2) = u(8); du(5) = u(11);
du(8) = DDq(2); du(11) = DDq(5);
du(3) = u(9); du(6) = u(12);
du(9) = DDq(3); du(12) = DDq(6);
du=du';
return;
c4science · Help