Homec4science
Diffusion MSM (master) Browse

msm/ShoulderModel_HHT/inverse_dynamic_simulation.m
8406fed2a624master

Last Change
Raw File

inverse_dynamic_simulation.m
View Options

function IDDATA = inverse_dynamic_simulation()
% ----------------------------------------------------------------------
% anatomical data
% muscle originis and insertion in [mm]:
AD_Ao=1e-3*[32 29 -15]; AD_Ai=1e-3*[0 -90 20];
MD_Ao=1e-3*[0 33 2]; MD_Ai=1e-3*[0 -90 20];
PD_Ao=1e-3*[-28 23 -55]; PD_Ai=1e-3*[0 -90 20];
SS_Ao=1e-3*[0 25 -81]; SS_Ai=1e-3*[0 7 26];
IS_Ao=1e-3*[-15 -22 -97]; IS_Ai=1e-3*[-21 8 5];
SC_Ao=1e-3*[2 -23 -99]; SC_Ai=1e-3*[21 5 5];
MusIOMat=cell2mat({AD_Ao AD_Ai;MD_Ao MD_Ai;PD_Ao PD_Ai;SS_Ao SS_Ai;...
IS_Ao IS_Ai;SC_Ao SC_Ai});
r=1e-3*[30 30 30 26.9 23 22.2];%different radii for the muscles crossing the humeral head
IDDATA.StepSize = 0.1;
t=[0.01:IDDATA.StepSize:7.2];%time sedgment
TC=20;%how fast the arm moves
IDDATA.t = t; % keeping the time somewhere for possible use elsewhere
for j=1:length(t)
disp(['solving inverse dynamic simulation, Time Stamp : ', num2str(j), '/', num2str(size(IDDATA.t,2))]);
% defining the kinematic (q, Dq, DDq) to impose to the model (q=phi, theta, psi)
phi=0;%((pi/8)*(cos(2*pi*t(j)/TC)-1));
Dphi=0;%(-0.25*(pi^2/TC)*sin(2*pi*t(j)/TC));
DDphi=0;%(-0.5*(pi^3/(TC^2))*cos(2*pi*t(j)/TC));
theta_abs=(pi/2)*(cos(2*pi*t(j)/TC)-1);%
Dtheta_abs=-(pi^2/TC)*sin(2*pi*t(j)/TC);%
DDtheta_abs=-2*(pi^3/(TC^2))*cos(2*pi*t(j)/TC);%
psi=((pi/4)*(cos(2*pi*t(j)/TC)-1));%
Dpsi=(-(2/4)*(pi^2/TC)*sin(2*pi*t(j)/TC));%-
DDpsi=(-(4/4)*(pi^3/(TC^2))*cos(2*pi*t(j)/TC));%-
IDDATA.ImposedMotion(:,j) = [psi Dpsi DDpsi theta_abs Dtheta_abs...
DDtheta_abs phi Dphi DDphi]';
%scapula motion is restricted to humerus by scapulohumeral rhythm
if ge(theta_abs,-pi/15)
theta_s=0;
else
theta_s=(1/1.8)*(theta_abs+pi/15);
end
%here are the rotation matrices. S,H, and T stand for scapula, humerus, and
%thorax, respectively. Thorax plays the role of the ground (base).
Rot_S2T = [1 0 0;
0 cos(theta_s) -sin(theta_s);
0 sin(theta_s) cos(theta_s)];
Rot_T2S = Rot_S2T';
R_phi = [cos(phi) 0 sin(phi);
0 1 0;
-sin(phi) 0 cos(phi)]; % around the b2
R_tht = [1 0 0;
0 cos(theta_abs) -sin(theta_abs);
0 sin(theta_abs) cos(theta_abs)]; % around the b1
R_psi = [cos(psi) 0 sin(psi);
0 1 0;
-sin(psi) 0 cos(psi)]; % around the b2
Rot_H2T = R_phi*R_tht*R_psi; % rotation matrix from humerus to thorax frame
Rot_T2H = Rot_H2T';
PartialVel=[sin(phi)*sin(theta_abs) cos(theta_abs) cos(phi)*sin(theta_abs);...
cos(phi) 0 sin(phi); 0 1 0]; % e_i
% Wrapping
%wrapping and here is the inner loop for different muscles
for i=1:6
Ao(:,i)=Rot_S2T*[MusIOMat(i,[1:3])]';
sinalpha(i)=Ao(2,i)/sqrt(Ao(1,i)^2+Ao(2,i)^2);
cosalpha(i)=Ao(1,i)/sqrt(Ao(1,i)^2+Ao(2,i)^2);
sinbeta(i)=(Ao(1,i)*cosalpha(i)+Ao(2,i)*sinalpha(i))/norm(Ao(:,i),2);
cosbeta(i)=Ao(3,i)/norm(Ao(:,i),2);
Ai_inT(:,i)=Rot_H2T*MusIOMat(i,[4:6])';
Ai_inTcop(:,i)=[Ai_inT(1,i)*cosbeta(i)*cosalpha(i)+Ai_inT(2,i)*cosbeta(i)*sinalpha(i)-Ai_inT(3,i)*sinbeta(i);
-Ai_inT(1,i)*sinalpha(i)+Ai_inT(2,i)*cosalpha(i);
Ai_inT(1,i)*sinbeta(i)*cosalpha(i)+Ai_inT(2,i)*sinbeta(i)*sinalpha(i)+Ai_inT(3,i)*cosbeta(i)];
singamma(i)=Ai_inTcop(2,i)/sqrt(Ai_inTcop(1,i)^2+Ai_inTcop(2,i)^2);
cosgamma(i)=Ai_inTcop(1,i)/sqrt(Ai_inTcop(1,i)^2+Ai_inTcop(2,i)^2);
RotMat_C2T([3*i-2:3*i],[1:3])=[cosgamma(i)*cosbeta(i)*cosalpha(i)-singamma(i)*sinalpha(i) -singamma(i)*cosbeta(i)*cosalpha(i)-cosgamma(i)*sinalpha(i) sinbeta(i)*cosalpha(i);
cosgamma(i)*cosbeta(i)*sinalpha(i)+singamma(i)*cosalpha(i) -singamma(i)*cosbeta(i)*sinalpha(i)+cosgamma(i)*cosalpha(i) sinbeta(i)*sinalpha(i);
-cosgamma(i)*sinbeta(i) singamma(i)*sinbeta(i) cosbeta(i)];
cosksi(i)=r(i)/norm(Ao(:,i),2);
AiAo(:,i)=Ao(:,i)-Ai_inT(:,i);
d(:,i)=(1/norm(AiAo(:,i),2))*AiAo(:,i);
digits(7)
DELTA(i)=vpa((d(1,i)*Ao(1,i)+d(2,i)*Ao(2,i)+d(3,i)*Ao(3,i))^2-...
(norm(Ao(:,i),2)^2-r(i)^2));
tt(i)=((Ao(1,i)-Ai_inT(1,i))*Ao(1,i)+(Ao(2,i)-Ai_inT(2,i))*Ao(2,i)...
+(Ao(3,i)-Ai_inT(3,i))*Ao(3,i))/(...
(Ai_inT(1,i)-Ao(1,i))^2+(Ai_inT(2,i)-Ao(2,i))^2+...
(Ai_inT(3,i)-Ao(3,i))^2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%wrapping condition
if ge(DELTA(i),0)&gt(tt(i),0)&lt(tt(i),1)%muscle should be wrapped.
T0_inC(:,i)=[r(i)*sqrt(1-cosksi(i)^2);0;r(i)*cosksi(i)]; % appication point T0
T0_inT(:,i)=RotMat_C2T([3*i-2:3*i],[1:3])*T0_inC(:,i);
Dir_inT(:,i)=(1/norm(Ao(:,i)-T0_inT(:,i),2))*(Ao(:,i)-T0_inT(:,i)); % direction n
else%wrapping is not needed.
T0_inT(:,i)=[Ao(1,i)+(Ai_inT(1,i)-Ao(1,i))*tt(i);
Ao(2,i)+(Ai_inT(2,i)-Ao(2,i))*tt(i);
Ao(3,i)+(Ai_inT(3,i)-Ao(3,i))*tt(i)];
Dir_inT(:,i)=(1/norm(Ao(:,i)-Ai_inT(:,i),2))*(Ao(:,i)-Ai_inT(:,i));
end
Rho_inT(:,i)=T0_inT(:,i);
Dir_inH(:,i)=Rot_T2H*Dir_inT(:,i);
Rho_inH(:,i)=Rot_T2H*Rho_inT(:,i);
end
IDDATA.LeverArm{j} = Rho_inT(:,:);
IDDATA.ForceDirection{j} = Dir_inT(:,:);
% Dynamics
%Dynamic of the system:
m = 3.82; %[N]
g = 9.81;
r_OCG=0.32; %[m]
I_b2b2=0.024; %[Kg/m2]
I_b1b1=0.003+0.3914; %[Kg/m2]
I_b3b3=0.003+0.3914; %[Kg/m2]
PCSAMat = 1e-2*diag([10 10 10 5 15 15]);
E = PCSAMat^-2;
E=(E+E')/2;
% Phi/theta/psi directions respectively the same as the partial velocities
AngVel=[Dphi*sin(psi)*sin(theta_abs)+Dtheta_abs*cos(psi);
Dphi*cos(theta_abs)+Dpsi;
-Dphi*cos(psi)*sin(theta_abs)+Dtheta_abs*sin(psi)]; % angular velocity in the humerus coordinate
AngAcc(:,j)=[DDphi*sin(psi)*sin(theta_abs)+Dphi*Dpsi*cos(psi)*sin(theta_abs)+Dphi*Dtheta_abs*sin(psi)*cos(theta_abs)+...
DDtheta_abs*cos(psi)-Dtheta_abs*Dpsi*sin(psi);...
DDphi*cos(theta_abs)-Dphi*Dtheta_abs*sin(theta_abs)+DDpsi;...
-DDphi*cos(psi)*sin(theta_abs)+Dphi*Dpsi*sin(psi)*sin(theta_abs)-Dphi*Dtheta_abs*cos(psi)*cos(theta_abs)+...
DDtheta_abs*sin(psi)+Dtheta_abs*Dpsi*cos(psi)]; % angular acceleration in the humerus frame
SysDyn=PartialVel*[I_b1b1 0 0;0 I_b2b2 0;0 0 I_b3b3]*...
AngAcc(:,j)-PartialVel*cross(Rot_H2T*[0;-r_OCG;0],[0;-m*g;0]); %system in YXY
% Save the generalized forces
IDDATA.GeneralizedForces(:,j) = SysDyn;
Acc_CG=0+cross(AngAcc(:,j),Rot_H2T*[0;-r_OCG;0])+cross(AngVel,cross(AngVel,Rot_H2T*[0;-r_OCG;0]))+0+0;
W=PartialVel*[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))];
%load-sharing
F_p=inv(E)*W'*inv(W*inv(E)*W')*SysDyn; % THE particular answer of the system
F_max=[40*10;40*10;40*10;40*5;40*15;40*15];
F_pp(:,j)=(100./F_max).*F_p; % relative particular answers for plotting
Null_W=null(W);
P=(Null_W'*E*Null_W) + (Null_W'*E*Null_W)'; % 2*(Null_W'*E*Null_W) originally
% but ger rid of the Hessian error
% due to its insymmetricity
q=(2*F_p'*E*Null_W)';
G=[Null_W;-Null_W];
h=[F_max-F_p;F_p];
opts=optimset('Algorithm','interior-point-convex','Display','off');
Mu=quadprog(P,q,G,h,[],[],[],[],[],opts);
F_ss=(100./F_max).*(Null_W*Mu); % special answers for plotting
IDDATA.MuscleForces(:,j)=F_p+Null_W*Mu; % final answers including both special and particular answers
IDDATA.JointReaction(:,j) = m*Acc_CG-[0;-m*g;0]-(Dir_inT*IDDATA.MuscleForces(:,j));%joint reaction force
IDDATA.JointReactionMagnitude(j)=norm(IDDATA.JointReaction(:,j)); % joint reaction force magnitude
end
figure
plot(IDDATA.t(1,:),IDDATA.MuscleForces(:,:))
return
c4science · Help