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)>(tt(i),0)<(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